From fd714611f520761f4aa1cc0dba449df58bae2d71 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Tue, 29 Oct 2019 18:12:39 +0100
Subject: [PATCH] Format
---
doc/additionalDoc/codeorg.h | 2 +-
doc/additionalDoc/e-operators.h | 4 +-
doc/additionalDoc/feature-doc.h | 2 +-
doc/additionalDoc/install-doc.h | 17 +-
doc/additionalDoc/namespace-doc.h | 1 -
doc/additionalDoc/package.h | 131 +-
doc/additionalDoc/parameter-server-doc.h | 2 +-
doc/additionalDoc/sot-doc.h | 2 +-
.../core/abstract-sot-external-interface.hh | 77 +-
include/sot/core/additional-functions.hh | 25 +-
include/sot/core/api.hh | 14 +-
include/sot/core/binary-int-to-uint.hh | 41 +-
include/sot/core/binary-op.hh | 107 +-
include/sot/core/causal-filter.hh | 116 +-
include/sot/core/clamp-workspace.hh | 73 +-
include/sot/core/com-freezer.hh | 67 +-
include/sot/core/constraint.hh | 74 +-
include/sot/core/contiifstream.hh | 29 +-
include/sot/core/control-gr.hh | 94 +-
include/sot/core/control-pd.hh | 121 +-
include/sot/core/debug.hh | 379 +--
include/sot/core/derivator-impl.hh | 45 +-
include/sot/core/derivator.hh | 168 +-
include/sot/core/device.hh | 309 +-
include/sot/core/event.hh | 169 +-
include/sot/core/exception-abstract.hh | 140 +-
include/sot/core/exception-dynamic.hh | 51 +-
include/sot/core/exception-factory.hh | 59 +-
include/sot/core/exception-feature.hh | 40 +-
include/sot/core/exception-signal.hh | 55 +-
include/sot/core/exception-task.hh | 49 +-
include/sot/core/exception-tools.hh | 65 +-
include/sot/core/exp-moving-avg.hh | 32 +-
include/sot/core/factory.hh | 8 +-
include/sot/core/feature-1d.hh | 68 +-
include/sot/core/feature-abstract.hh | 517 ++--
include/sot/core/feature-generic.hh | 67 +-
include/sot/core/feature-joint-limits.hh | 72 +-
include/sot/core/feature-line-distance.hh | 69 +-
include/sot/core/feature-point6d-relative.hh | 64 +-
include/sot/core/feature-point6d.hh | 108 +-
include/sot/core/feature-pose.hh | 129 +-
include/sot/core/feature-posture.hh | 98 +-
include/sot/core/feature-task.hh | 49 +-
include/sot/core/feature-vector3.hh | 65 +-
include/sot/core/feature-visual-point.hh | 68 +-
include/sot/core/filter-differentiator.hh | 175 +-
include/sot/core/fir-filter-impl.hh | 46 +-
include/sot/core/fir-filter.hh | 460 ++-
include/sot/core/flags.hh | 121 +-
include/sot/core/gain-adaptive.hh | 98 +-
include/sot/core/gain-hyperbolic.hh | 91 +-
include/sot/core/gradient-ascent.hh | 32 +-
include/sot/core/gripper-control.hh | 110 +-
include/sot/core/integrator-abstract-impl.hh | 42 +-
include/sot/core/integrator-abstract.hh | 95 +-
include/sot/core/integrator-euler-impl.hh | 50 +-
include/sot/core/integrator-euler.hh | 145 +-
include/sot/core/joint-limitator.hh | 79 +-
include/sot/core/joint-trajectory-entity.hh | 78 +-
include/sot/core/kalman.hh | 223 +-
include/sot/core/latch.hh | 109 +-
include/sot/core/macros-signal.hh | 241 +-
include/sot/core/madgwickahrs.hh | 163 +-
include/sot/core/mailbox-vector.hh | 30 +-
include/sot/core/mailbox.hh | 51 +-
include/sot/core/mailbox.hxx | 217 +-
include/sot/core/matrix-constant.hh | 45 +-
include/sot/core/matrix-geometry.hh | 166 +-
include/sot/core/matrix-svd.hh | 28 +-
include/sot/core/memory-task-sot.hh | 77 +-
include/sot/core/motion-period.hh | 66 +-
include/sot/core/multi-bound.hh | 71 +-
include/sot/core/neck-limitation.hh | 69 +-
include/sot/core/op-point-modifier.hh | 64 +-
include/sot/core/parameter-server.hh | 157 +-
include/sot/core/periodic-call-entity.hh | 48 +-
include/sot/core/periodic-call.hh | 60 +-
include/sot/core/pool.hh | 47 +-
include/sot/core/reader.hh | 71 +-
include/sot/core/robot-simu.hh | 51 +-
include/sot/core/robot-utils.hh | 385 +--
include/sot/core/seq-play.hh | 80 +-
include/sot/core/seqplay.hh | 81 +-
include/sot/core/sequencer.hh | 153 +-
include/sot/core/smooth-reach.hh | 84 +-
include/sot/core/sot.hh | 413 ++-
include/sot/core/stop-watch.hh | 77 +-
include/sot/core/switch.hh | 114 +-
include/sot/core/task-abstract.hh | 103 +-
include/sot/core/task-conti.hh | 58 +-
include/sot/core/task-pd.hh | 50 +-
include/sot/core/task-unilateral.hh | 54 +-
include/sot/core/task.hh | 84 +-
include/sot/core/time-stamp.hh | 64 +-
include/sot/core/timer.hh | 187 +-
include/sot/core/trajectory.hh | 189 +-
include/sot/core/unary-op.hh | 101 +-
include/sot/core/utils-windows.hh | 7 +-
include/sot/core/variadic-op.hh | 292 +-
include/sot/core/vector-constant.hh | 44 +-
include/sot/core/vector-to-rotation.hh | 54 +-
include/sot/core/visual-point-projecter.hh | 63 +-
src/control/control-gr.cpp | 126 +-
src/control/control-pd.cpp | 121 +-
src/debug/contiifstream.cpp | 54 +-
src/debug/debug.cpp | 74 +-
src/exception/exception-abstract.cpp | 99 +-
src/exception/exception-dynamic.cpp | 28 +-
src/exception/exception-factory.cpp | 43 +-
src/exception/exception-feature.cpp | 27 +-
src/exception/exception-signal.cpp | 28 +-
src/exception/exception-task.cpp | 27 +-
src/exception/exception-tools.cpp | 27 +-
src/factory/additional-functions.cpp | 55 +-
src/factory/pool.cpp | 298 +-
src/feature/feature-1d.cpp | 82 +-
src/feature/feature-abstract.cpp | 149 +-
src/feature/feature-generic.cpp | 151 +-
src/feature/feature-joint-limits-command.h | 68 +-
src/feature/feature-joint-limits.cpp | 178 +-
src/feature/feature-line-distance.cpp | 325 +-
src/feature/feature-point6d-relative.cpp | 373 +--
src/feature/feature-point6d.cpp | 661 ++--
src/feature/feature-pose.cpp | 329 +-
src/feature/feature-posture.cpp | 290 +-
src/feature/feature-task.cpp | 22 +-
src/feature/feature-vector3.cpp | 134 +-
src/feature/feature-visual-point.cpp | 200 +-
src/feature/visual-point-projecter.cpp | 168 +-
src/filters/causal-filter.cpp | 131 +-
src/filters/filter-differentiator.cpp | 328 +-
src/filters/madgwickahrs.cpp | 466 ++-
src/math/op-point-modifier.cpp | 200 +-
src/math/vector-quaternion.cpp | 182 +-
src/matrix/derivator.cpp | 68 +-
src/matrix/fir-filter.cpp | 102 +-
src/matrix/integrator-abstract.cpp | 13 +-
src/matrix/integrator-euler.cpp | 18 +-
src/matrix/integrator-euler.t.cpp | 63 +-
src/matrix/matrix-constant-command.h | 77 +-
src/matrix/matrix-constant.cpp | 54 +-
src/matrix/matrix-svd.cpp | 58 +-
src/matrix/operator.cpp | 2090 +++++++------
src/matrix/vector-constant-command.h | 68 +-
src/matrix/vector-constant.cpp | 54 +-
src/matrix/vector-to-rotation.cpp | 93 +-
src/signal/signal-cast.cpp | 396 ++-
src/sot/flags.cpp | 661 ++--
src/sot/memory-task-sot.cpp | 90 +-
src/sot/rotation-simple.cpp | 6 +-
src/sot/solver-hierarchical-inequalities.cpp | 2726 +++++++++--------
src/sot/sot-command.h | 415 ++-
src/sot/sot-h.cpp | 504 ++-
src/sot/sot-qr.cpp | 1290 ++++----
src/sot/sot.cpp | 1204 ++++----
src/sot/weighted-sot.cpp | 728 ++---
src/task/constraint-command.h | 88 +-
src/task/constraint.cpp | 128 +-
src/task/gain-adaptive.cpp | 195 +-
src/task/gain-hyperbolic.cpp | 115 +-
src/task/multi-bound.cpp | 386 ++-
src/task/task-abstract.cpp | 24 +-
src/task/task-command.h | 77 +-
src/task/task-conti.cpp | 114 +-
src/task/task-pd.cpp | 87 +-
src/task/task-unilateral.cpp | 69 +-
src/task/task.cpp | 385 ++-
src/tools/binary-int-to-uint.cpp | 35 +-
src/tools/clamp-workspace.cpp | 169 +-
src/tools/com-freezer.cpp | 54 +-
src/tools/device.cpp | 648 ++--
src/tools/event.cpp | 33 +-
src/tools/exp-moving-avg.cpp | 58 +-
src/tools/gradient-ascent.cpp | 45 +-
src/tools/gripper-control.cpp | 197 +-
src/tools/joint-limitator.cpp | 94 +-
src/tools/joint-trajectory-command.hh | 58 +-
src/tools/joint-trajectory-entity.cpp | 330 +-
src/tools/kalman.cpp | 333 +-
src/tools/latch.cpp | 8 +-
src/tools/mailbox-vector.cpp | 8 +-
src/tools/motion-period.cpp | 118 +-
src/tools/neck-limitation.cpp | 185 +-
src/tools/parameter-server.cpp | 582 ++--
src/tools/periodic-call-entity.cpp | 47 +-
src/tools/periodic-call.cpp | 216 +-
src/tools/robot-simu.cpp | 58 +-
src/tools/robot-utils-py.cpp | 96 +-
src/tools/robot-utils.cpp | 993 +++---
src/tools/seq-play.cpp | 118 +-
src/tools/sequencer.cpp | 345 +--
src/tools/smooth-reach.cpp | 156 +-
src/tools/smooth-reach.hpp | 80 +-
src/tools/switch.cpp | 26 +-
src/tools/time-stamp.cpp | 81 +-
src/tools/timer.cpp | 61 +-
src/tools/trajectory.cpp | 379 +--
src/tools/type-name-helper.hh | 38 +-
src/tools/utils-windows.cpp | 66 +-
src/traces/reader.cpp | 178 +-
src/utils/stop-watch.cpp | 304 +-
unitTesting/control/test_control_pd.cpp | 14 +-
unitTesting/dummy.cpp | 3 +-
unitTesting/factory/test_factory.cpp | 158 +-
unitTesting/features/test_feature_generic.cpp | 723 +++--
unitTesting/features/test_feature_point6d.cpp | 142 +-
.../filters/test_filter_differentiator.cpp | 60 +-
unitTesting/filters/test_madgwick_ahrs.cpp | 32 +-
unitTesting/filters/test_madgwick_arhs.cpp | 54 +-
unitTesting/math/matrix-homogeneous.cpp | 138 +-
unitTesting/math/matrix-twist.cpp | 250 +-
unitTesting/matrix/test_operator.cpp | 200 +-
unitTesting/python/CMakeLists.txt | 2 +-
unitTesting/python/op-point-modifier.py | 18 +-
unitTesting/signal/test_dep.cpp | 207 +-
unitTesting/signal/test_depend.cpp | 203 +-
unitTesting/signal/test_ptr.cpp | 121 +-
unitTesting/signal/test_ptrcast.cpp | 14 +-
unitTesting/signal/test_signal.cpp | 85 +-
unitTesting/sot/test_solverSoth.cpp | 853 +++---
unitTesting/sot/tsot.cpp | 55 +-
unitTesting/task/test_flags.cpp | 90 +-
unitTesting/task/test_gain.cpp | 37 +-
unitTesting/task/test_multi_bound.cpp | 57 +-
unitTesting/task/test_task.cpp | 56 +-
unitTesting/tools/plugin.cc | 44 +-
unitTesting/tools/plugin.hh | 11 +-
unitTesting/tools/test_abstract_interface.cpp | 47 +-
unitTesting/tools/test_boost.cpp | 349 ++-
unitTesting/tools/test_control_pd.cpp | 9 +-
unitTesting/tools/test_device.cpp | 37 +-
unitTesting/tools/test_mailbox.cpp | 38 +-
unitTesting/tools/test_matrix.cpp | 65 +-
unitTesting/tools/test_robot_utils.cpp | 136 +-
unitTesting/traces/files.cpp | 19 +-
unitTesting/traces/test_traces.cpp | 42 +-
237 files changed, 18641 insertions(+), 20170 deletions(-)
diff --git a/doc/additionalDoc/codeorg.h b/doc/additionalDoc/codeorg.h
index 4462f499..9eaeb694 100644
--- a/doc/additionalDoc/codeorg.h
+++ b/doc/additionalDoc/codeorg.h
@@ -1,7 +1,7 @@
/** \page codeorganization Code organization
\section sec_organization Organization of the code
-
+
The code is based on the <c>dynamic-graph</c> package, which provides the
framework on which sot-core relies. Hence most of the code in <c>sot-core</c>
consists of classes that derive from dynamic_graph::Entity.
diff --git a/doc/additionalDoc/e-operators.h b/doc/additionalDoc/e-operators.h
index 2ac68dd7..743f6213 100644
--- a/doc/additionalDoc/e-operators.h
+++ b/doc/additionalDoc/e-operators.h
@@ -5,7 +5,7 @@
\subsection ssubp_unitary_op Vector selector
-This entity output a vector remapping an input vector
+This entity output a vector remapping an input vector
\f${\bf v}_{in}=[v_0,v_1,\cdots,v_n]\f$.
It is realized by specifying bounds such as \f$({[i,j],[k,l]})\f$,
then the output vector will be the contanetion of the
@@ -13,7 +13,7 @@ intervals extracted from the input vector:
\f${\bf v}_{out}=[v_i,v_{i+1},\cdots,v_{j-1},v_{j},v_{k},
v_{k+1},\cdots,v_{l-1},v_l]\f$
For instance if we have an input vector
-such that:
+such that:
\code
1
2
diff --git a/doc/additionalDoc/feature-doc.h b/doc/additionalDoc/feature-doc.h
index 50dec82f..4adaabe8 100644
--- a/doc/additionalDoc/feature-doc.h
+++ b/doc/additionalDoc/feature-doc.h
@@ -10,7 +10,7 @@
<ul>
<li> Feature1D : \f$ L_2 \f$ norm of \f$ e(t)\f$
dynamicgraph::sot::Feature1D</li>
- <li> FeatureGeneric : Externalisation of the Feature computation
+ <li> FeatureGeneric : Externalisation of the Feature computation
dynamicgraph::sot::FeatureGeneric </li>
<li> FeatureJointLimits : Distance to the joint limits and its Jacobian
dynamicgraph::sot::FeatureJointLimits </li>
diff --git a/doc/additionalDoc/install-doc.h b/doc/additionalDoc/install-doc.h
index c2d81abb..e1c46bca 100644
--- a/doc/additionalDoc/install-doc.h
+++ b/doc/additionalDoc/install-doc.h
@@ -10,7 +10,7 @@ This library is based on several packages:
\li <c>pthread</c>
\li <c>python</c>
\li <c>pinocchio</c> (https://github.com/stack-of-tasks/pinocchio)
-
+
Their presence will be checked with the use of pkg-config.
\section sec_installation Installation
@@ -23,15 +23,12 @@ extension</a>.
A short and quick way to get it on 16.04 LTS is to do the following:
\verbatim
sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
-deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub kinetic robotpkg
-deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub kinetic robotpkg
-EOF
-\endverbatim
-This created a file to add robotpkg and robotpkg/wip as apt repositories.
-To identify the packages from this repository the server key must be added:
-\verbatim
-curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key |
- sudo apt-key add -
+deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub kinetic
+robotpkg deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub
+kinetic robotpkg EOF \endverbatim This created a file to add robotpkg and
+robotpkg/wip as apt repositories. To identify the packages from this repository
+the server key must be added: \verbatim curl
+http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
\endverbatim
The list of available packages is provided through:
\verbatim
diff --git a/doc/additionalDoc/namespace-doc.h b/doc/additionalDoc/namespace-doc.h
index 49e338a4..861e001f 100644
--- a/doc/additionalDoc/namespace-doc.h
+++ b/doc/additionalDoc/namespace-doc.h
@@ -7,4 +7,3 @@
This is the namespace for a subset of helperd classes related to the
implementation of the Stack-Of-Tasks.
*/
-
diff --git a/doc/additionalDoc/package.h b/doc/additionalDoc/package.h
index 10869526..79b33ab4 100644
--- a/doc/additionalDoc/package.h
+++ b/doc/additionalDoc/package.h
@@ -9,7 +9,7 @@ having high level software capabilities, like plugins and python scripting.
The package includes the following functionnalities:
<ul>
<li> A parameter server object to register and collect information on your
-robot (see \subpage page_parameter_server) </li>
+robot (see \subpage page_parameter_server) </li>
<li> From the internal SoT point of view a class called Device which is
allowing a direct interaction with your robot.</li>
<li> From the extern SoT point of view a class which makes the connection
@@ -17,9 +17,9 @@ either with the hardware or with a simulator </li>
<li> A full kinematic engine implemented in an incremental manner.</li>
</ul>
-\subpage page_RequirementsInstallation
+\subpage page_RequirementsInstallation
-\subpage page_featuredoc
+\subpage page_featuredoc
\subpage page_sot
@@ -52,10 +52,9 @@ A hierarchical inequality solver, sot::SolverHierarchicalInequalities, is
present. For the maths, see \cite Mansard2007.
\subsection subsec_exceptions Exceptions
-The library defines the following classes of exceptions that derive from std::exception:
-\li sot::ExceptionAbstract (the base class for all exceptions in sot-core; use it to
-catch only sot-core exceptions)
-\li sot::ExceptionDynamic
+The library defines the following classes of exceptions that derive from
+std::exception: \li sot::ExceptionAbstract (the base class for all exceptions in
+sot-core; use it to catch only sot-core exceptions) \li sot::ExceptionDynamic
\li sot::ExceptionFactory (raised if issues in instancing tasks and features)
\li sot::ExceptionFeature (raised by a feature - see \ref subsec_Features)
\li sot::ExceptionSignal
@@ -78,7 +77,7 @@ Tasks are the hierarchical element of the stack of tasks.
A task computes a value and a Jacobian as output signals.
Once stacked into a solver, the solver will compute the control vector
-that makes the task value converge toward zero in the
+that makes the task value converge toward zero in the
order defined by the priority levels. For more information see the documentation
of class dynamicgraph::sot:TaskAbstract.
@@ -87,8 +86,9 @@ The following classes encapsulate common mathematical objects, and
are all defined in the core library; see each individual class documentation
for reference.
\li Vectors: (sot::VectorQuaternion, sot::VectorRollPitchYaw, sot::VectorUTheta)
-\li Matrices: (sot::MatrixForce, sot::MatrixHomogeneous, sot::MatrixRotation, sot::MatrixTwist)
-\li sot::MultiBound can be used to enforce bounds on numeric values
+\li Matrices: (sot::MatrixForce, sot::MatrixHomogeneous, sot::MatrixRotation,
+sot::MatrixTwist) \li sot::MultiBound can be used to enforce bounds on numeric
+values
\subsection subsec_others Others
The core library also contains functions for adaptation and extension of
@@ -101,61 +101,61 @@ See \ref factory for additional information.
\defgroup plugins_list List of python modules
These python modules are linked with the core library.
- sot/sot-qr
- sot/weighted-sot
- sot/sot-h
- sot/sot
-
- math/op-point-modifier
-
- matrix/binary-op
- matrix/derivator
- matrix/fir-filter
- matrix/integrator-abstract
- matrix/integrator-euler
- matrix/matrix-constant
- matrix/unary-op
- matrix/vector-constant
- matrix/vector-to-rotation
-
- task/gain-adaptive
- task/task-pd
- task/constraint
- task/gain-hyperbolic
- task/task
- task/task-conti
- task/task-unilateral
-
- feature/feature-point6d
- feature/feature-vector3
- feature/feature-generic
- feature/feature-joint-limits
- feature/feature-1d
- feature/feature-point6d-relative
- feature/feature-visual-point
- feature/feature-task
- feature/feature-line-distance
-
- traces/reader
-
- tools/time-stamp
- tools/timer
- tools/seq-play
- tools/sequencer
- tools/robot-simu
- tools/periodic-call-entity
- tools/motion-period
- tools/neck-limitation
- tools/mailbox-vector
- tools/kalman
- tools/joint-limitator
- tools/gripper-control
- tools/com-freezer
- tools/clamp-workspace
- tools/binary-int-to-uint
-
- control/control-gr
- control/control-pd
+ sot/sot-qr
+ sot/weighted-sot
+ sot/sot-h
+ sot/sot
+
+ math/op-point-modifier
+
+ matrix/binary-op
+ matrix/derivator
+ matrix/fir-filter
+ matrix/integrator-abstract
+ matrix/integrator-euler
+ matrix/matrix-constant
+ matrix/unary-op
+ matrix/vector-constant
+ matrix/vector-to-rotation
+
+ task/gain-adaptive
+ task/task-pd
+ task/constraint
+ task/gain-hyperbolic
+ task/task
+ task/task-conti
+ task/task-unilateral
+
+ feature/feature-point6d
+ feature/feature-vector3
+ feature/feature-generic
+ feature/feature-joint-limits
+ feature/feature-1d
+ feature/feature-point6d-relative
+ feature/feature-visual-point
+ feature/feature-task
+ feature/feature-line-distance
+
+ traces/reader
+
+ tools/time-stamp
+ tools/timer
+ tools/seq-play
+ tools/sequencer
+ tools/robot-simu
+ tools/periodic-call-entity
+ tools/motion-period
+ tools/neck-limitation
+ tools/mailbox-vector
+ tools/kalman
+ tools/joint-limitator
+ tools/gripper-control
+ tools/com-freezer
+ tools/clamp-workspace
+ tools/binary-int-to-uint
+
+ control/control-gr
+ control/control-pd
@defgroup factory Factory
@@ -185,4 +185,3 @@ They are all located in the tests directory.
*/
-
diff --git a/doc/additionalDoc/parameter-server-doc.h b/doc/additionalDoc/parameter-server-doc.h
index 834da845..c761550f 100644
--- a/doc/additionalDoc/parameter-server-doc.h
+++ b/doc/additionalDoc/parameter-server-doc.h
@@ -3,5 +3,5 @@
In order to register and recover information on your robot it is possible
to register information using the class dynamicgraph::sot::RobotUtil.
-
+
*/
diff --git a/doc/additionalDoc/sot-doc.h b/doc/additionalDoc/sot-doc.h
index 5a3950d8..27345ee1 100644
--- a/doc/additionalDoc/sot-doc.h
+++ b/doc/additionalDoc/sot-doc.h
@@ -1,5 +1,5 @@
/** \page page_sot Stack-of-Tasks
-
+
As explained in class dynamicgraph::sot::FeatureAbstract,
\f[
{\bf E}(t) = {\bf e}({\bf q}(t), t)= {\bf s}({\bf q}(t)) - {\bf s}^*(t)
diff --git a/include/sot/core/abstract-sot-external-interface.hh b/include/sot/core/abstract-sot-external-interface.hh
index 18c1fca7..ef94adbc 100644
--- a/include/sot/core/abstract-sot-external-interface.hh
+++ b/include/sot/core/abstract-sot-external-interface.hh
@@ -9,63 +9,60 @@
#ifndef ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
#define ABSTRACT_SOT_EXTERNAL_INTERFACE_HH
-#include <vector>
#include <map>
-#include <string>
#include <sot/core/api.hh>
+#include <string>
+#include <vector>
namespace dynamicgraph {
- namespace sot {
-
- class SOT_CORE_EXPORT NamedVector
- {
-
- private:
- std::string name_;
- std::vector<double> values_;
+namespace sot {
- public:
- NamedVector() {}
- ~NamedVector() {}
+class SOT_CORE_EXPORT NamedVector {
- const std::string & getName() const
- { return name_;}
+private:
+ std::string name_;
+ std::vector<double> values_;
- void setName(const std::string & aname)
- { name_ = aname;}
+public:
+ NamedVector() {}
+ ~NamedVector() {}
- const std::vector<double> & getValues() const
- { return values_;}
+ const std::string &getName() const { return name_; }
- void setValues(const std::vector<double> & values)
- { values_ = values;}
+ void setName(const std::string &aname) { name_ = aname; }
- };
- typedef NamedVector SensorValues;
- typedef NamedVector ControlValues;
+ const std::vector<double> &getValues() const { return values_; }
- class SOT_CORE_EXPORT AbstractSotExternalInterface
- {
- public:
+ void setValues(const std::vector<double> &values) { values_ = values; }
+};
+typedef NamedVector SensorValues;
+typedef NamedVector ControlValues;
- AbstractSotExternalInterface(){}
+class SOT_CORE_EXPORT AbstractSotExternalInterface {
+public:
+ AbstractSotExternalInterface() {}
- virtual ~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;
- };
- }
-}
+ virtual void getControl(std::map<std::string, ControlValues> &) = 0;
+ virtual void setSecondOrderIntegration(void) = 0;
+ virtual void setNoIntegration(void) = 0;
+};
+} // namespace sot
+} // namespace dynamicgraph
-typedef dynamicgraph::sot::AbstractSotExternalInterface * createSotExternalInterface_t();
-typedef void destroySotExternalInterface_t (dynamicgraph::sot::AbstractSotExternalInterface *);
+typedef dynamicgraph::sot::AbstractSotExternalInterface *
+createSotExternalInterface_t();
+typedef void destroySotExternalInterface_t(
+ dynamicgraph::sot::AbstractSotExternalInterface *);
#endif
diff --git a/include/sot/core/additional-functions.hh b/include/sot/core/additional-functions.hh
index cb2effac..c4f82e00 100644
--- a/include/sot/core/additional-functions.hh
+++ b/include/sot/core/additional-functions.hh
@@ -8,34 +8,35 @@
*/
/* SOT */
+#include "sot/core/api.hh"
+#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-base.h>
#include <sot/core/exception-factory.hh>
-#include <dynamic-graph/pool.h>
#include <sot/core/pool.hh>
-#include "sot/core/api.hh"
/* --- STD --- */
-#include <string>
#include <map>
#include <sstream>
+#include <string>
/* --- BOOST --- */
-#include <boost/function.hpp>
#include <boost/bind.hpp>
+#include <boost/function.hpp>
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/*! @ingroup factory
\brief This helper class dynamically overloads the "new" shell command
to allow creation of tasks and features as well as entities.
*/
-class AdditionalFunctions
-{
+class AdditionalFunctions {
public:
- AdditionalFunctions();
- ~AdditionalFunctions();
- static void cmdFlagSet( const std::string& cmd,std::istringstream& args,
- std::ostream& os );
+ AdditionalFunctions();
+ ~AdditionalFunctions();
+ static void cmdFlagSet(const std::string &cmd, std::istringstream &args,
+ std::ostream &os);
};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
diff --git a/include/sot/core/api.hh b/include/sot/core/api.hh
index 28e3e01f..bf357141 100644
--- a/include/sot/core/api.hh
+++ b/include/sot/core/api.hh
@@ -10,14 +10,14 @@
#ifndef SOT_CORE_API_HH
#define SOT_CORE_API_HH
-#if defined (WIN32)
-# ifdef sot_core_EXPORTS
-# define SOT_CORE_EXPORT __declspec(dllexport)
-# else
-# define SOT_CORE_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#ifdef sot_core_EXPORTS
+#define SOT_CORE_EXPORT __declspec(dllexport)
#else
-# define SOT_CORE_EXPORT
+#define SOT_CORE_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOT_CORE_EXPORT
#endif
#endif
diff --git a/include/sot/core/binary-int-to-uint.hh b/include/sot/core/binary-int-to-uint.hh
index 366175c8..e61879f1 100644
--- a/include/sot/core/binary-int-to-uint.hh
+++ b/include/sot/core/binary-int-to-uint.hh
@@ -15,51 +15,48 @@
/* --------------------------------------------------------------------- */
/* SOT */
+#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/exception-task.hh>
-#include <dynamic-graph/all-signals.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (binary_int_to_uint_EXPORTS)
-# define SOTBINARYINTTOUINT_EXPORT __declspec(dllexport)
-# else
-# define SOTBINARYINTTOUINT_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(binary_int_to_uint_EXPORTS)
+#define SOTBINARYINTTOUINT_EXPORT __declspec(dllexport)
+#else
+#define SOTBINARYINTTOUINT_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTBINARYINTTOUINT_EXPORT
+#define SOTBINARYINTTOUINT_EXPORT
#endif
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint
- : public dg::Entity
-{
+class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dg::Entity {
public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
/* --- SIGNALS ------------------------------------------------------------ */
public:
-
- dg::SignalPtr< int,int > binaryIntSIN;
- dg::SignalTimeDependent< unsigned,int > binaryUintSOUT;
+ dg::SignalPtr<int, int> binaryIntSIN;
+ dg::SignalTimeDependent<unsigned, int> binaryUintSOUT;
public:
- BinaryIntToUint( const std::string& name );
+ BinaryIntToUint(const std::string &name);
virtual ~BinaryIntToUint() {}
- virtual unsigned& computeOutput( unsigned& res,int time );
+ virtual unsigned &computeOutput(unsigned &res, int time);
- virtual void display( std::ostream& os ) const;
+ virtual void display(std::ostream &os) const;
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif
diff --git a/include/sot/core/binary-op.hh b/include/sot/core/binary-op.hh
index a741d729..43d0277a 100644
--- a/include/sot/core/binary-op.hh
+++ b/include/sot/core/binary-op.hh
@@ -18,11 +18,11 @@
#include <dynamic-graph/linear-algebra.h>
/* SOT */
-#include <sot/core/flags.hh>
-#include <dynamic-graph/entity.h>
-#include <sot/core/pool.hh>
#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>
@@ -30,61 +30,56 @@
#include <boost/function.hpp>
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- 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;
+namespace sot {
- 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(); }
- static const std::string CLASS_NAME;
- virtual const std::string& getClassName () const { return CLASS_NAME; }
- std::string getDocString () const { return op.getDocString ();}
-
- BinaryOp( const std::string& name )
- : Entity(name)
- ,SIN1(NULL,BinaryOp::CLASS_NAME+"("+name+")::input("+getTypeIn1Name()+")::sin1")
- ,SIN2(NULL,CLASS_NAME+"("+name+")::input("+getTypeIn2Name()+")::sin2")
- ,SOUT( boost::bind(&Self::computeOperation,this,_1,_2),
- SIN1<<SIN2,CLASS_NAME+"("+name+")::output("+getTypeOutName()+")::sout")
- {
- signalRegistration( SIN1<<SIN2<<SOUT );
- op.addSpecificCommands(*this,commandMap);
- }
-
- virtual ~BinaryOp( void ) {};
-
- public: /* --- SIGNAL --- */
-
- SignalPtr<Tin1,int> SIN1;
- SignalPtr<Tin2,int> SIN2;
- SignalTimeDependent<Tout,int> SOUT;
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
- protected:
- Tout& computeOperation( Tout& res,int time )
- {
- const Tin1 &x1 = SIN1(time);
- const Tin2 &x2 = SIN2(time);
- op(x1,x2,res);
- return res;
- }
- };
- } // namespace sot
+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 --- */
+ static std::string getTypeIn1Name(void) { return Operator::nameTypeIn1(); }
+ static std::string getTypeIn2Name(void) { return Operator::nameTypeIn2(); }
+ static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName() const { return CLASS_NAME; }
+ std::string getDocString() const { return op.getDocString(); }
+
+ BinaryOp(const std::string &name)
+ : Entity(name),
+ SIN1(NULL, BinaryOp::CLASS_NAME + "(" + name + ")::input(" +
+ getTypeIn1Name() + ")::sin1"),
+ SIN2(NULL, CLASS_NAME + "(" + name + ")::input(" + getTypeIn2Name() +
+ ")::sin2"),
+ SOUT(boost::bind(&Self::computeOperation, this, _1, _2), SIN1 << SIN2,
+ CLASS_NAME + "(" + name + ")::output(" + getTypeOutName() +
+ ")::sout") {
+ signalRegistration(SIN1 << SIN2 << SOUT);
+ op.addSpecificCommands(*this, commandMap);
+ }
+
+ virtual ~BinaryOp(void){};
+
+public: /* --- SIGNAL --- */
+ SignalPtr<Tin1, int> SIN1;
+ SignalPtr<Tin2, int> SIN2;
+ SignalTimeDependent<Tout, int> SOUT;
+
+protected:
+ Tout &computeOperation(Tout &res, int time) {
+ const Tin1 &x1 = SIN1(time);
+ const Tin2 &x2 = SIN2(time);
+ op(x1, x2, res);
+ return res;
+ }
+};
+} // namespace sot
} // namespace dynamicgraph
-
#endif // #ifndef SOT_CORE_BINARYOP_HH
diff --git a/include/sot/core/causal-filter.hh b/include/sot/core/causal-filter.hh
index 66fd8dab..8f3110d5 100644
--- a/include/sot/core/causal-filter.hh
+++ b/include/sot/core/causal-filter.hh
@@ -16,8 +16,6 @@
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
-
-
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -26,69 +24,67 @@
/** \addtogroup Filters
\section subsec_causalfilter CausalFilter
Filter data with an IIR or FIR filter.
-
- Filter a data sequence, \f$x\f$, using a digital filter.
- The filter is a direct form II transposed implementation
+
+ Filter a data sequence, \f$x\f$, using a digital filter.
+ The filter is a direct form II transposed implementation
of the standard difference equation.
This means that the filter implements:
-
+
\f$ a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
- a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)] \f$
-
- where \f$m\f$ is the degree of the numerator,
- \f$n\f$ is the degree of the denominator,
+
+ where \f$m\f$ is the degree of the numerator,
+ \f$n\f$ is the degree of the denominator,
and \f$N\f$ is the sample number
-
-
+
+
*/
namespace dynamicgraph {
- namespace sot {
-
- class CausalFilter
- {
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /** --- CONSTRUCTOR ----
- \param[in] timestep
- \param[in] xSize
- \param[in] filter_numerator
- \param[in] filter_denominator
-
- xSize is
- */
- CausalFilter(const double ×tep,
- const int& xSize,
- const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator);
-
- void get_x_dx_ddx(const Eigen::VectorXd& base_x,
- Eigen::VectorXd& x_output_dx_ddx);
-
- void switch_filter(const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator);
-
- private:
- /// sampling timestep of the input signal
- double m_dt;
- /// Size
- int m_x_size;
- /// Size of the numerator \f$m\f$
- Eigen::VectorXd::Index m_filter_order_m;
- /// Size of the denominator \f$n\f$
- Eigen::VectorXd::Index m_filter_order_n;
-
- /// Coefficients of the numerator \f$b\f$
- Eigen::VectorXd m_filter_numerator;
- /// Coefficients of the denominator \f$a\f$
- Eigen::VectorXd m_filter_denominator;
- bool m_first_sample;
- ///
- int m_pt_numerator;
- int m_pt_denominator;
- Eigen::MatrixXd m_input_buffer;
- Eigen::MatrixXd m_output_buffer;
- }; // class CausalFilter
- } /// core
-} /// sot
+namespace sot {
+
+class CausalFilter {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /** --- CONSTRUCTOR ----
+ \param[in] timestep
+ \param[in] xSize
+ \param[in] filter_numerator
+ \param[in] filter_denominator
+
+ xSize is
+ */
+ CausalFilter(const double ×tep, const int &xSize,
+ const Eigen::VectorXd &filter_numerator,
+ const Eigen::VectorXd &filter_denominator);
+
+ void get_x_dx_ddx(const Eigen::VectorXd &base_x,
+ Eigen::VectorXd &x_output_dx_ddx);
+
+ void switch_filter(const Eigen::VectorXd &filter_numerator,
+ const Eigen::VectorXd &filter_denominator);
+
+private:
+ /// sampling timestep of the input signal
+ double m_dt;
+ /// Size
+ int m_x_size;
+ /// Size of the numerator \f$m\f$
+ Eigen::VectorXd::Index m_filter_order_m;
+ /// Size of the denominator \f$n\f$
+ Eigen::VectorXd::Index m_filter_order_n;
+
+ /// Coefficients of the numerator \f$b\f$
+ Eigen::VectorXd m_filter_numerator;
+ /// Coefficients of the denominator \f$a\f$
+ Eigen::VectorXd m_filter_denominator;
+ bool m_first_sample;
+ ///
+ int m_pt_numerator;
+ int m_pt_denominator;
+ Eigen::MatrixXd m_input_buffer;
+ Eigen::MatrixXd m_output_buffer;
+}; // 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 0fa65e87..d82905ed 100644
--- a/include/sot/core/clamp-workspace.hh
+++ b/include/sot/core/clamp-workspace.hh
@@ -18,64 +18,59 @@
namespace dg = dynamicgraph;
/* SOT */
+#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/exception-task.hh>
-#include <dynamic-graph/all-signals.h>
#include <sot/core/matrix-geometry.hh>
-
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (clamp_workspace_EXPORTS)
-# define SOTCLAMPWORKSPACE_EXPORT __declspec(dllexport)
-# else
-# define SOTCLAMPWORKSPACE_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(clamp_workspace_EXPORTS)
+#define SOTCLAMPWORKSPACE_EXPORT __declspec(dllexport)
+#else
+#define SOTCLAMPWORKSPACE_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTCLAMPWORKSPACE_EXPORT
+#define SOTCLAMPWORKSPACE_EXPORT
#endif
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace
- : public dg::Entity
-{
- public:
+class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace : public dg::Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
/* --- SIGNALS ------------------------------------------------------------ */
- public:
+public:
+ dg::SignalPtr<MatrixHomogeneous, int> positionrefSIN;
+ dg::SignalPtr<MatrixHomogeneous, int> positionSIN;
+ dg::SignalTimeDependent<dg::Matrix, int> alphaSOUT;
+ dg::SignalTimeDependent<dg::Matrix, int> alphabarSOUT;
+ dg::SignalTimeDependent<MatrixHomogeneous, int> handrefSOUT;
- dg::SignalPtr< MatrixHomogeneous,int > positionrefSIN;
- dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
- dg::SignalTimeDependent< dg::Matrix,int > alphaSOUT;
- dg::SignalTimeDependent< dg::Matrix,int > alphabarSOUT;
- dg::SignalTimeDependent< MatrixHomogeneous,int > handrefSOUT;
+public:
+ ClampWorkspace(const std::string &name);
+ virtual ~ClampWorkspace(void) {}
- public:
+ void update(int time);
- ClampWorkspace( const std::string& name );
- virtual ~ClampWorkspace( void ) {}
+ virtual dg::Matrix &computeOutput(dg::Matrix &res, int time);
+ virtual dg::Matrix &computeOutputBar(dg::Matrix &res, int time);
+ virtual MatrixHomogeneous &computeRef(MatrixHomogeneous &res, int time);
- void update( int time );
-
- virtual dg::Matrix& computeOutput( dg::Matrix& res, int time );
- virtual dg::Matrix& computeOutputBar( dg::Matrix& res, int time );
- virtual MatrixHomogeneous& computeRef( MatrixHomogeneous& res, int time );
-
- virtual void display( std::ostream& ) const;
-
- private:
+ virtual void display(std::ostream &) const;
+private:
int timeUpdate;
dg::Matrix alpha;
@@ -95,16 +90,12 @@ class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace
double theta_max;
int mode;
- enum {
- FRAME_POINT,
- FRAME_REF
- } frame;
+ enum { FRAME_POINT, FRAME_REF } frame;
- std::pair<double,double> bounds[3];
+ std::pair<double, double> bounds[3];
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif
diff --git a/include/sot/core/com-freezer.hh b/include/sot/core/com-freezer.hh
index 283016d0..6e4a7aee 100644
--- a/include/sot/core/com-freezer.hh
+++ b/include/sot/core/com-freezer.hh
@@ -19,25 +19,25 @@
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (com_freezer_EXPORTS)
-# define SOTCOMFREEZER_EXPORT __declspec(dllexport)
-# else
-# define SOTCOMFREEZER_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(com_freezer_EXPORTS)
+#define SOTCOMFREEZER_EXPORT __declspec(dllexport)
#else
-# define SOTCOMFREEZER_EXPORT
+#define SOTCOMFREEZER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTCOMFREEZER_EXPORT
#endif
-
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
@@ -45,38 +45,33 @@ namespace dg = dynamicgraph;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-class SOTCOMFREEZER_EXPORT CoMFreezer
-: public dg::Entity
-{
- public:
- static const std::string CLASS_NAME;
- virtual const std::string & getClassName() const { return CLASS_NAME; }
+class SOTCOMFREEZER_EXPORT CoMFreezer : public dg::Entity {
+public:
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName() const { return CLASS_NAME; }
- private:
- dg::Vector m_lastCoM;
- bool m_previousPGInProcess;
- int m_lastStopTime;
+private:
+ dg::Vector m_lastCoM;
+ bool m_previousPGInProcess;
+ int m_lastStopTime;
- public: /* --- CONSTRUCTION --- */
- CoMFreezer(const std::string & name);
- virtual ~CoMFreezer(void);
+public: /* --- CONSTRUCTION --- */
+ CoMFreezer(const std::string &name);
+ virtual ~CoMFreezer(void);
- public: /* --- SIGNAL --- */
- dg::SignalPtr<dg::Vector, int> CoMRefSIN;
- dg::SignalPtr<unsigned, int> PGInProcessSIN;
- dg::SignalTimeDependent<dg::Vector, int> freezedCoMSOUT;
+public: /* --- SIGNAL --- */
+ dg::SignalPtr<dg::Vector, int> CoMRefSIN;
+ dg::SignalPtr<unsigned, int> PGInProcessSIN;
+ dg::SignalTimeDependent<dg::Vector, int> freezedCoMSOUT;
- public: /* --- FUNCTION --- */
- dg::Vector& computeFreezedCoM(dg::Vector & freezedCoM, const int& time);
+public: /* --- FUNCTION --- */
+ dg::Vector &computeFreezedCoM(dg::Vector &freezedCoM, const int &time);
- public: /* --- PARAMS --- */
- virtual void display(std::ostream & os) const;
+public: /* --- PARAMS --- */
+ virtual void display(std::ostream &os) const;
};
-
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SOTCOMFREEZER_H_H */
diff --git a/include/sot/core/constraint.hh b/include/sot/core/constraint.hh
index 919964d5..57e9442e 100644
--- a/include/sot/core/constraint.hh
+++ b/include/sot/core/constraint.hh
@@ -22,65 +22,61 @@ namespace dg = dynamicgraph;
#include <string>
/* SOT */
+#include <dynamic-graph/all-signals.h>
+#include <sot/core/exception-signal.hh>
+#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>
-#include <dynamic-graph/all-signals.h>
-#include <sot/core/exception-task.hh>
-#include <sot/core/exception-signal.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (constraint_EXPORTS)
-# define SOTCONSTRAINT_EXPORT __declspec(dllexport)
-# else
-# define SOTCONSTRAINT_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(constraint_EXPORTS)
+#define SOTCONSTRAINT_EXPORT __declspec(dllexport)
#else
-# define SOTCONSTRAINT_EXPORT
+#define SOTCONSTRAINT_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTCONSTRAINT_EXPORT
#endif
-
namespace dynamicgraph {
- namespace sot {
+namespace sot {
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
- class SOTCONSTRAINT_EXPORT Constraint
- : public TaskAbstract
- {
- protected:
- typedef std::list< Signal<dg::Matrix,int>* > JacobianList;
- JacobianList jacobianList;
+class SOTCONSTRAINT_EXPORT Constraint : public TaskAbstract {
+protected:
+ typedef std::list<Signal<dg::Matrix, int> *> JacobianList;
+ JacobianList jacobianList;
- public:
- static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+public:
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- public:
- Constraint( const std::string& n );
+public:
+ Constraint(const std::string &n);
- void addJacobian( Signal<dg::Matrix,int>& sig );
- void clearJacobianList( void );
+ void addJacobian(Signal<dg::Matrix, int> &sig);
+ void clearJacobianList(void);
- void setControlSelection( const Flags& act );
- void addControlSelection( const Flags& act );
- void clearControlSelection( void );
+ void setControlSelection(const Flags &act);
+ void addControlSelection(const Flags &act);
+ void clearControlSelection(void);
- /* --- COMPUTATION --- */
- dg::Matrix& computeJacobian( dg::Matrix& J,int time );
+ /* --- COMPUTATION --- */
+ dg::Matrix &computeJacobian(dg::Matrix &J, int time);
- /* --- DISPLAY ------------------------------------------------------------ */
- SOTCONSTRAINT_EXPORT friend std::ostream& operator<< ( std::ostream& os,const Constraint& t );
- };
- } // namespace sot
+ /* --- DISPLAY ------------------------------------------------------------ */
+ SOTCONSTRAINT_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const Constraint &t);
+};
+} // namespace sot
} // namespace dynamicgraph
-
-
#endif /* #ifndef __SOT_CONSTRAINT_H__ */
diff --git a/include/sot/core/contiifstream.hh b/include/sot/core/contiifstream.hh
index e0e429c0..3a3ebe82 100644
--- a/include/sot/core/contiifstream.hh
+++ b/include/sot/core/contiifstream.hh
@@ -14,7 +14,6 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
#include <fstream>
#include <sstream>
#ifndef WIN32
@@ -27,36 +26,38 @@
#include <pthread.h>
#endif
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-class SOT_CORE_EXPORT Contiifstream
-{
+class SOT_CORE_EXPORT Contiifstream {
protected:
std::string filename;
std::streamoff cursor;
static const unsigned int BUFFER_SIZE = 256;
char buffer[BUFFER_SIZE];
- std::list< std::string > reader;
+ std::list<std::string> reader;
bool first;
public: /* --- Constructor --- */
- Contiifstream( const std::string& n="" );
- ~Contiifstream( void );
- void open( const std::string& n ) { filename=n; cursor=0; }
+ Contiifstream(const std::string &n = "");
+ ~Contiifstream(void);
+ void open(const std::string &n) {
+ filename = n;
+ cursor = 0;
+ }
public: /* --- READ FILE --- */
- bool loop( void );
+ bool loop(void);
public: /* --- READ LIST --- */
- inline bool ready( void ) { return 0<reader.size();}
- std::string next( void ) ;
-
-
+ inline bool ready(void) { return 0 < reader.size(); }
+ std::string next(void);
};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_CONTIIFSTREAM_HH__ */
diff --git a/include/sot/core/control-gr.hh b/include/sot/core/control-gr.hh
index 34f3f3ba..0ba768e9 100644
--- a/include/sot/core/control-gr.hh
+++ b/include/sot/core/control-gr.hh
@@ -19,80 +19,68 @@
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (control_gr_EXPORTS)
-# define ControlGR_EXPORT __declspec(dllexport)
-# else
-# define ControlGR_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(control_gr_EXPORTS)
+#define ControlGR_EXPORT __declspec(dllexport)
+#else
+#define ControlGR_EXPORT __declspec(dllimport)
+#endif
#else
-# define ControlGR_EXPORT
+#define ControlGR_EXPORT
#endif
namespace dynamicgraph {
- namespace sot {
-
- namespace dg = dynamicgraph;
-
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class ControlGR_EXPORT ControlGR
- : public Entity
- {
+namespace sot {
- public: /* --- CONSTRUCTOR ---- */
-
- ControlGR( const std::string & name );
-
- public: /* --- INIT --- */
-
- void init( const double& step);
-
- public: /* --- CONSTANTS --- */
-
- /* Default values. */
- static const double TIME_STEP_DEFAULT; // = 0.001
-
- 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; }
+namespace dg = dynamicgraph;
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
- protected:
+class ControlGR_EXPORT ControlGR : public Entity {
- /* Parameters of the torque-control function:
- * tau = - A*qddot = g */
- double TimeStep;
- double _dimension;
+public: /* --- CONSTRUCTOR ---- */
+ ControlGR(const std::string &name);
- public: /* --- SIGNALS --- */
+public: /* --- INIT --- */
+ void init(const double &step);
- SignalPtr<dg::Matrix,int> matrixASIN;
- SignalPtr<dg::Vector,int> accelerationSIN;
- SignalPtr<dg::Vector,int> gravitySIN;
- SignalTimeDependent<dg::Vector,int> controlSOUT;
+public: /* --- CONSTANTS --- */
+ /* Default values. */
+ static const double TIME_STEP_DEFAULT; // = 0.001
- protected:
+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; }
- double& setsize(int dimension);
- dg::Vector& computeControl( dg::Vector& tau,int t );
+protected:
+ /* Parameters of the torque-control function:
+ * tau = - A*qddot = g */
+ double TimeStep;
+ double _dimension;
- };
+public: /* --- SIGNALS --- */
+ SignalPtr<dg::Matrix, int> matrixASIN;
+ SignalPtr<dg::Vector, int> accelerationSIN;
+ SignalPtr<dg::Vector, int> gravitySIN;
+ SignalTimeDependent<dg::Vector, int> controlSOUT;
+protected:
+ double &setsize(int dimension);
+ dg::Vector &computeControl(dg::Vector &tau, int t);
+};
- } // namespace sot
+} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __SOT_Control_GR_HH__
diff --git a/include/sot/core/control-pd.hh b/include/sot/core/control-pd.hh
index 35a918a6..a9bc3576 100644
--- a/include/sot/core/control-pd.hh
+++ b/include/sot/core/control-pd.hh
@@ -19,88 +19,73 @@
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/entity.h>
-
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (control_pd_EXPORTS)
-# define ControlPD_EXPORT __declspec(dllexport)
-# else
-# define ControlPD_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(control_pd_EXPORTS)
+#define ControlPD_EXPORT __declspec(dllexport)
+#else
+#define ControlPD_EXPORT __declspec(dllimport)
+#endif
#else
-# define ControlPD_EXPORT
+#define ControlPD_EXPORT
#endif
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class ControlPD_EXPORT ControlPD
- : public Entity
- {
-
- public: /* --- CONSTRUCTOR ---- */
-
- ControlPD( const std::string & name );
-
- public: /* --- INIT --- */
-
- void init( const double& step);
-
- public: /* --- CONSTANTS --- */
-
- /* Default values. */
- static const double TIME_STEP_DEFAULT; // = 0.001
-
- 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:
-
- /* Parameters of the torque-control function:
- * tau = kp * (qd-q) + kd* (dqd-dq) */
- double TimeStep;
-
- public: /* --- SIGNALS --- */
-
- SignalPtr<dg::Vector,int> KpSIN;
- SignalPtr<dg::Vector,int> KdSIN;
- SignalPtr<dg::Vector,int> positionSIN;
- SignalPtr<dg::Vector,int> desiredpositionSIN;
- SignalPtr<dg::Vector,int> velocitySIN;
- SignalPtr<dg::Vector,int> desiredvelocitySIN;
- SignalTimeDependent<dg::Vector,int> controlSOUT;
- SignalTimeDependent<dg::Vector,int> positionErrorSOUT;
- SignalTimeDependent<dg::Vector,int> velocityErrorSOUT;
-
- protected:
-
- dg::Vector& computeControl( dg::Vector& tau,int t );
- dg::Vector position_error_;
- dg::Vector velocity_error_;
- dg::Vector& getPositionError( dg::Vector& position_error,int t );
- dg::Vector& getVelocityError( dg::Vector& velocity_error,int t );
-
- };
+namespace sot {
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+class ControlPD_EXPORT ControlPD : public Entity {
+
+public: /* --- CONSTRUCTOR ---- */
+ ControlPD(const std::string &name);
+
+public: /* --- INIT --- */
+ void init(const double &step);
+
+public: /* --- CONSTANTS --- */
+ /* Default values. */
+ static const double TIME_STEP_DEFAULT; // = 0.001
+
+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:
+ /* Parameters of the torque-control function:
+ * tau = kp * (qd-q) + kd* (dqd-dq) */
+ double TimeStep;
+
+public: /* --- SIGNALS --- */
+ SignalPtr<dg::Vector, int> KpSIN;
+ SignalPtr<dg::Vector, int> KdSIN;
+ SignalPtr<dg::Vector, int> positionSIN;
+ SignalPtr<dg::Vector, int> desiredpositionSIN;
+ SignalPtr<dg::Vector, int> velocitySIN;
+ SignalPtr<dg::Vector, int> desiredvelocitySIN;
+ SignalTimeDependent<dg::Vector, int> controlSOUT;
+ SignalTimeDependent<dg::Vector, int> positionErrorSOUT;
+ SignalTimeDependent<dg::Vector, int> velocityErrorSOUT;
+
+protected:
+ dg::Vector &computeControl(dg::Vector &tau, int t);
+ dg::Vector position_error_;
+ dg::Vector velocity_error_;
+ dg::Vector &getPositionError(dg::Vector &position_error, int t);
+ dg::Vector &getVelocityError(dg::Vector &velocity_error, int t);
+};
} // namespace sot
} // namespace dynamicgraph
-
-
#endif // #ifndef __SOT_Control_PD_HH__
diff --git a/include/sot/core/debug.hh b/include/sot/core/debug.hh
index 5f52facf..dee51e55 100644
--- a/include/sot/core/debug.hh
+++ b/include/sot/core/debug.hh
@@ -8,214 +8,215 @@
*/
#ifndef SOT_CORE_DEBUG_HH
-# define SOT_CORE_DEBUG_HH
-# include <cstdio>
-# include <cstdarg>
-# include <fstream>
-# include <sstream>
-# include "sot/core/api.hh"
-
-# ifndef VP_DEBUG_MODE
-# define VP_DEBUG_MODE 0
-# 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; \
- } while(0)
-
-namespace dynamicgraph
-{
- namespace sot
- {
- class SOT_CORE_EXPORT DebugTrace
- {
- public:
- static const int SIZE = 512;
-
- std::stringstream tmpbuffer;
- std::ostream& outputbuffer;
- char charbuffer[SIZE+1];
- int traceLevel;
- int traceLevelTemplate;
-
- DebugTrace( std::ostream& os ): outputbuffer(os) {}
-
- inline void trace(const int level,const char* format, ...)
- {
- if (level<=traceLevel)
- SOT_COMMON_TRACES;
- tmpbuffer.str("");
- }
-
- inline void trace(const char* format,...)
- {
- SOT_COMMON_TRACES;
- tmpbuffer.str("");
- }
-
- inline void trace(const int level=-1)
- {
- if (level<=traceLevel)
- outputbuffer << tmpbuffer.str();
- tmpbuffer.str("");
- }
-
- inline void traceTemplate(const int level, const char* format, ...)
- {
- if (level<=traceLevelTemplate)
- SOT_COMMON_TRACES;
- tmpbuffer.str("");
- }
-
- inline void traceTemplate(const char* format, ...)
- {
- SOT_COMMON_TRACES;
- tmpbuffer.str("");
- }
-
- inline DebugTrace& pre(const std::ostream&)
- {
- return *this;
- }
-
- inline DebugTrace& pre(const std::ostream&, int level)
- {
- traceLevel = level;
- return *this;
- }
-
- static const char* DEBUG_FILENAME_DEFAULT;
- static void openFile(const char * filename = DEBUG_FILENAME_DEFAULT);
- static void closeFile(const char * filename = DEBUG_FILENAME_DEFAULT);
- };
-
- SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
- SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
- } // namespace sot
+#define SOT_CORE_DEBUG_HH
+#include "sot/core/api.hh"
+#include <cstdarg>
+#include <cstdio>
+#include <fstream>
+#include <sstream>
+
+#ifndef VP_DEBUG_MODE
+#define VP_DEBUG_MODE 0
+#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; \
+ } while (0)
+
+namespace dynamicgraph {
+namespace sot {
+class SOT_CORE_EXPORT DebugTrace {
+public:
+ static const int SIZE = 512;
+
+ std::stringstream tmpbuffer;
+ std::ostream &outputbuffer;
+ char charbuffer[SIZE + 1];
+ int traceLevel;
+ int traceLevelTemplate;
+
+ DebugTrace(std::ostream &os) : outputbuffer(os) {}
+
+ inline void trace(const int level, const char *format, ...) {
+ if (level <= traceLevel)
+ SOT_COMMON_TRACES;
+ tmpbuffer.str("");
+ }
+
+ inline void trace(const char *format, ...) {
+ SOT_COMMON_TRACES;
+ tmpbuffer.str("");
+ }
+
+ inline void trace(const int level = -1) {
+ if (level <= traceLevel)
+ outputbuffer << tmpbuffer.str();
+ tmpbuffer.str("");
+ }
+
+ inline void traceTemplate(const int level, const char *format, ...) {
+ if (level <= traceLevelTemplate)
+ SOT_COMMON_TRACES;
+ tmpbuffer.str("");
+ }
+
+ inline void traceTemplate(const char *format, ...) {
+ SOT_COMMON_TRACES;
+ tmpbuffer.str("");
+ }
+
+ inline DebugTrace &pre(const std::ostream &) { return *this; }
+
+ inline DebugTrace &pre(const std::ostream &, int level) {
+ traceLevel = level;
+ return *this;
+ }
+
+ static const char *DEBUG_FILENAME_DEFAULT;
+ static void openFile(const char *filename = DEBUG_FILENAME_DEFAULT);
+ static void closeFile(const char *filename = DEBUG_FILENAME_DEFAULT);
+};
+
+SOT_CORE_EXPORT extern DebugTrace sotDEBUGFLOW;
+SOT_CORE_EXPORT extern DebugTrace sotERRORFLOW;
+} // namespace sot
} // namespace dynamicgraph
-# ifdef VP_DEBUG
-# define sotPREDEBUG \
- __FILE__ << ": " <<__FUNCTION__ \
- << "(#" << __LINE__ << ") :"
-
-# define sotPREERROR \
- "\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
- << "(#" << __LINE__ << ") :"
-
-# 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 \
- dynamicgraph::sot::sotDEBUGFLOW.outputbuffer
-
-# 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).trace
-
-# define sotERRORF \
- if(!dynamicgraph::sot::sotDEBUGFLOW.outputbuffer.good()) \
- ; \
- else \
- sot::sotERRORFLOW.pre(sot::sotERRORFLOW.tmpbuffer<<sotPREERROR).trace
+#ifdef VP_DEBUG
+#define sotPREDEBUG \
+ __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :"
+
+#define sotPREERROR \
+ "\t!! " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :"
+
+#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 \
+ dynamicgraph::sot::sotDEBUGFLOW.outputbuffer
+
+#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) \
+ .trace
+
+#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 \
- 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).trace
+#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) \
+ .trace
namespace dynamicgraph {
- namespace sot {
- 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 sot {
+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
- /* -------------------------------------------------------------------------- */
-# else // VP_DEBUG
-# define sotPREERROR \
- "\t!! "<<__FILE__ << ": " <<__FUNCTION__ \
- << "(#" << __LINE__ << ") :"
-# define sotDEBUG(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
-# define sotDEBUGMUTE(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
-# define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR
+/* -------------------------------------------------------------------------- */
+#else // VP_DEBUG
+#define sotPREERROR \
+ "\t!! " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :"
+#define sotDEBUG(level) \
+ if (1) \
+ ; \
+ else \
+ ::dynamicgraph::sot::__null_stream()
+#define sotDEBUGMUTE(level) \
+ if (1) \
+ ; \
+ else \
+ ::dynamicgraph::sot::__null_stream()
+#define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR
namespace dynamicgraph {
- namespace sot {
- inline void sotDEBUGF(const int,const char*,...) {}
- inline void sotDEBUGF(const char*,...) {}
- inline void sotERRORF(const int, const char*,...) {}
- inline void sotERRORF(const char*,...) {}
- inline std::ostream& __null_stream () {
- // This function should never be called. With -O3,
- // it should not appear in the generated binary.
- static std::ostream os (NULL); return os;
- }
- } // namespace sot
+namespace sot {
+inline void sotDEBUGF(const int, const char *, ...) {}
+inline void sotDEBUGF(const char *, ...) {}
+inline void sotERRORF(const int, const char *, ...) {}
+inline void sotERRORF(const char *, ...) {}
+inline std::ostream &__null_stream() {
+ // This function should never be called. With -O3,
+ // it should not appear in the generated binary.
+ static std::ostream os(NULL);
+ return os;
+}
+} // namespace sot
} // namespace dynamicgraph
- // TEMPLATE
-# define sotTDEBUG(level) if( 1 ) ; else ::dynamicgraph::sot::__null_stream()
+// TEMPLATE
+#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 sot {
+inline void sotTDEBUGF(const int, const char *, ...) {}
+inline void sotTDEBUGF(const char *, ...) {}
+} // namespace sot
} // namespace dynamicgraph
-# define sotDEBUG_ENABLE(level) false
-# define sotTDEBUG_ENABLE(level) false
-
-# endif // VP_DEBUG
+#define sotDEBUG_ENABLE(level) false
+#define sotTDEBUG_ENABLE(level) false
-# define sotDEBUGIN(level) sotDEBUG(level) << "# In {" << std::endl
-# define sotDEBUGOUT(level) sotDEBUG(level) << "# Out }" << std::endl
-# define sotDEBUGINOUT(level) sotDEBUG(level) << "# In/Out { }" << std::endl
+#endif // VP_DEBUG
-# define sotTDEBUGIN(level) sotTDEBUG(level) << "# In {" << std::endl
-# define sotTDEBUGOUT(level) sotTDEBUG(level) << "# Out }" << std::endl
-# define sotTDEBUGINOUT(level) sotTDEBUG(level) << "# In/Out { }" << std::endl
+#define sotDEBUGIN(level) sotDEBUG(level) << "# In {" << std::endl
+#define sotDEBUGOUT(level) sotDEBUG(level) << "# Out }" << std::endl
+#define sotDEBUGINOUT(level) sotDEBUG(level) << "# In/Out { }" << std::endl
+#define sotTDEBUGIN(level) sotTDEBUG(level) << "# In {" << std::endl
+#define sotTDEBUGOUT(level) sotTDEBUG(level) << "# Out }" << std::endl
+#define sotTDEBUGINOUT(level) sotTDEBUG(level) << "# In/Out { }" << std::endl
#endif //! #ifdef SOT_CORE_DEBUG_HH
diff --git a/include/sot/core/derivator-impl.hh b/include/sot/core/derivator-impl.hh
index eab0bd3f..616b4cd7 100644
--- a/include/sot/core/derivator-impl.hh
+++ b/include/sot/core/derivator-impl.hh
@@ -17,41 +17,40 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (derivator_EXPORTS)
-# define DERIVATOR_EXPORT __declspec(dllexport)
-# else
-# define DERIVATOR_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(derivator_EXPORTS)
+#define DERIVATOR_EXPORT __declspec(dllexport)
#else
-# define DERIVATOR_EXPORT
+#define DERIVATOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define DERIVATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
#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) \
- typedef Derivator<sotSigType> className;
+#define DECLARE_SPECIFICATION(className, sotSigType) \
+ typedef Derivator<sotSigType> className;
#endif
-DECLARE_SPECIFICATION(DerivatorDouble,double)
-DECLARE_SPECIFICATION(DerivatorVector,dg::Vector)
-DECLARE_SPECIFICATION(DerivatorMatrix,dg::Matrix)
-DECLARE_SPECIFICATION(DerivatorVectorQuaternion,VectorQuaternion)
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+DECLARE_SPECIFICATION(DerivatorDouble, double)
+DECLARE_SPECIFICATION(DerivatorVector, dg::Vector)
+DECLARE_SPECIFICATION(DerivatorMatrix, dg::Matrix)
+DECLARE_SPECIFICATION(DerivatorVectorQuaternion, VectorQuaternion)
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_DERIVATOR_H__
diff --git a/include/sot/core/derivator.hh b/include/sot/core/derivator.hh
index bc7a9b5d..883c141a 100644
--- a/include/sot/core/derivator.hh
+++ b/include/sot/core/derivator.hh
@@ -18,99 +18,97 @@
#include <dynamic-graph/linear-algebra.h>
/* SOT */
-#include <sot/core/flags.hh>
-#include <sot/core/pool.hh>
-#include <dynamic-graph/entity.h>
#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>
namespace dynamicgraph {
- namespace sot {
- namespace dg = dynamicgraph;
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- template< class T >
- class Derivator : public dg::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
- protected:
- T memory;
- bool initialized;
- double timestep;
- static const double TIMESTEP_DEFAULT ; //= 1.;
-
- public: /* --- CONSTRUCTION --- */
-
- static std::string getTypeName( void ) { return "Unknown"; }
-
- Derivator( const std::string& name )
- : dg::Entity(name)
- ,memory(),initialized(false)
- ,timestep(TIMESTEP_DEFAULT)
- ,SIN(NULL,"sotDerivator<"+getTypeName()+">("+name+")::input("+getTypeName()+")::sin")
- ,SOUT( boost::bind(&Derivator<T>::computeDerivation,this,_1,_2),
- SIN,"sotDerivator<"+getTypeName()+">("+name+")::output("+getTypeName()+")::sout")
- ,timestepSIN("sotDerivator<"+getTypeName()+">("+name+")::input(double)::dt")
- {
- signalRegistration( SIN<<SOUT<<timestepSIN );
- timestepSIN.setReferenceNonConstant( ×tep );
- timestepSIN.setKeepReference(true);
- }
-
- virtual ~Derivator( void ) {};
-
- public: /* --- SIGNAL --- */
-
- dg::SignalPtr<T,int> SIN;
- dg::SignalTimeDependent<T,int> SOUT;
- dg::Signal<double,int> timestepSIN;
-
- protected:
- T& computeDerivation( T& res,int time )
- {
- if(initialized)
- {
- res = memory;
- res *= -1;
- memory = SIN(time);
- res += memory;
- if( timestep!=1. ) res *= (1./timestep);
- } else {
- initialized = true;
- memory = SIN(time);
- res = memory;
- res *= 0;
- }
- return res;
- }
- };
- //TODO Derivation of unit quaternion?
- template<>
- 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);
- } else {
- initialized = true;
- memory = SIN(time);
- res = memory;
- res.coeffs() *= 0;
- }
- return res;
- }
-
- } /* namespace sot */
-} /* namespace dynamicgraph */
+namespace sot {
+namespace dg = dynamicgraph;
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+template <class T> class Derivator : public dg::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+protected:
+ T memory;
+ bool initialized;
+ double timestep;
+ static const double TIMESTEP_DEFAULT; //= 1.;
+
+public: /* --- CONSTRUCTION --- */
+ static std::string getTypeName(void) { return "Unknown"; }
+
+ Derivator(const std::string &name)
+ : dg::Entity(name), memory(), initialized(false),
+ timestep(TIMESTEP_DEFAULT),
+ SIN(NULL, "sotDerivator<" + getTypeName() + ">(" + name + ")::input(" +
+ getTypeName() + ")::sin"),
+ SOUT(boost::bind(&Derivator<T>::computeDerivation, this, _1, _2), SIN,
+ "sotDerivator<" + getTypeName() + ">(" + name + ")::output(" +
+ getTypeName() + ")::sout"),
+ timestepSIN("sotDerivator<" + getTypeName() + ">(" + name +
+ ")::input(double)::dt") {
+ signalRegistration(SIN << SOUT << timestepSIN);
+ timestepSIN.setReferenceNonConstant(×tep);
+ timestepSIN.setKeepReference(true);
+ }
+
+ virtual ~Derivator(void){};
+
+public: /* --- SIGNAL --- */
+ dg::SignalPtr<T, int> SIN;
+ dg::SignalTimeDependent<T, int> SOUT;
+ dg::Signal<double, int> timestepSIN;
+
+protected:
+ T &computeDerivation(T &res, int time) {
+ if (initialized) {
+ res = memory;
+ res *= -1;
+ memory = SIN(time);
+ res += memory;
+ if (timestep != 1.)
+ res *= (1. / timestep);
+ } else {
+ initialized = true;
+ memory = SIN(time);
+ res = memory;
+ res *= 0;
+ }
+ return res;
+ }
+};
+// TODO Derivation of unit quaternion?
+template <>
+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);
+ } else {
+ initialized = true;
+ memory = SIN(time);
+ res = memory;
+ res.coeffs() *= 0;
+ }
+ return res;
+}
+
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_DERIVATOR_H__
diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh
index fe1e6693..4ff08119 100644
--- a/include/sot/core/device.hh
+++ b/include/sot/core/device.hh
@@ -19,169 +19,160 @@
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
+#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"
namespace dynamicgraph {
- namespace sot {
-
- /// Define the type of input expected by the robot
- enum ControlInput
- {
- CONTROL_INPUT_NO_INTEGRATION=0,
- CONTROL_INPUT_ONE_INTEGRATION=1,
- CONTROL_INPUT_TWO_INTEGRATION=2,
- CONTROL_INPUT_SIZE=3
- };
-
- const std::string ControlInput_s[] =
- {
- "noInteg", "oneInteg", "twoInteg"
- };
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class SOT_CORE_EXPORT Device
- :public Entity
- {
- public:
- static const std::string CLASS_NAME;
- virtual const std::string& getClassName(void) const {
- return CLASS_NAME;
- }
-
- enum ForceSignalSource
- {
- FORCE_SIGNAL_RLEG,
- FORCE_SIGNAL_LLEG,
- FORCE_SIGNAL_RARM,
- FORCE_SIGNAL_LARM
- };
-
- protected:
- dg::Vector state_;
- dg::Vector velocity_;
- bool sanityCheck_;
- dg::Vector vel_control_;
- ControlInput controlInputType_;
- bool withForceSignals[4];
- PeriodicCall periodicCallBefore_;
- PeriodicCall periodicCallAfter_;
- double timestep_;
-
- /// \name Robot bounds used for sanity checks
- /// \{
- Vector upperPosition_;
- Vector upperVelocity_;
- Vector upperTorque_;
- Vector lowerPosition_;
- Vector lowerVelocity_;
- Vector lowerTorque_;
- /// \}
- public:
-
- /* --- CONSTRUCTION --- */
- Device(const std::string& name);
- /* --- DESTRUCTION --- */
- virtual ~Device();
-
- virtual void setStateSize(const unsigned int& size);
- virtual void setState(const dg::Vector& st);
- void setVelocitySize(const unsigned int& size);
- virtual void setVelocity(const dg::Vector & vel);
- virtual void setSecondOrderIntegration();
- virtual void setNoIntegration();
- virtual void setControlInputType(const std::string& cit);
- virtual void increment(const double & dt = 5e-2);
-
- /// \name Sanity check parameterization
- /// \{
- void setSanityCheck (const bool & enableCheck);
- void setPositionBounds(const Vector& lower, const Vector& upper);
- void setVelocityBounds(const Vector& lower, const Vector& upper);
- void setTorqueBounds (const Vector& lower, const Vector& upper);
- /// \}
-
- public: /* --- DISPLAY --- */
- virtual void display(std::ostream& os) const;
- virtual void cmdDisplay();
- SOT_CORE_EXPORT friend std::ostream&
- operator<<(std::ostream& os,const Device& r) {
- r.display(os); return os;
- }
-
- public: /* --- SIGNALS --- */
-
- dynamicgraph::SignalPtr<dg::Vector,int> controlSIN;
- dynamicgraph::SignalPtr<dg::Vector,int> attitudeSIN;
- dynamicgraph::SignalPtr<dg::Vector,int> zmpSIN;
-
- /// \name Device current state.
- /// \{
- dynamicgraph::Signal<dg::Vector,int> stateSOUT;
- dynamicgraph::Signal<dg::Vector,int> velocitySOUT;
- dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
- /*! \brief The current state of the robot from the command viewpoint. */
- dynamicgraph::Signal<dg::Vector,int> motorcontrolSOUT;
- dynamicgraph::Signal<dg::Vector,int> previousControlSOUT;
- /*! \brief The ZMP reference send by the previous controller. */
- dynamicgraph::Signal<dg::Vector,int> ZMPPreviousControllerSOUT;
- /// \}
-
- /// \name Real robot current state
- /// This corresponds to the real encoders values and take into
- /// account the stabilization step. Therefore, this usually
- /// does *not* match the state control input signal.
- /// \{
- /// Motor positions
- dynamicgraph::Signal<dg::Vector, int> robotState_;
- /// Motor velocities
- dynamicgraph::Signal<dg::Vector, int> robotVelocity_;
- /// The force torque sensors
- dynamicgraph::Signal<dg::Vector,int>* forcesSOUT[4];
- /// Motor torques
- /// \todo why pseudo ?
- dynamicgraph::Signal<dg::Vector,int> pseudoTorqueSOUT;
- /// \}
-
- protected:
- /// Compute roll pitch yaw angles of freeflyer joint.
- void integrateRollPitchYaw(dg::Vector& state, const dg::Vector& control,
- double dt);
- /// Store Position of free flyer joint
- MatrixHomogeneous ffPose_;
- /// Compute the new position, from the current control.
- ///
- /// When sanity checks are enabled, this checks that the control is within
- /// bounds. There are three cases, depending on what the control is:
- /// - position: checks that the position is within bounds,
- /// - velocity: checks that the velocity and the future position are
- /// within bounds,
- /// - acceleration: checks that the acceleration, the future velocity and
- /// position are within bounds.
- /// \todo in order to check the acceleration, we need
- /// pinocchio and the contact forces in order to estimate
- /// the joint torques for the given acceleration.
- virtual void integrate( const double & dt );
- protected:
- /// Get freeflyer pose
- const MatrixHomogeneous& freeFlyerPose() const;
- public:
- virtual void setRoot( const dg::Matrix & root );
-
-
- virtual void setRoot( const MatrixHomogeneous & worldMwaist );
- private:
- // Intermediate variable to avoid dynamic allocation
- dg::Vector forceZero6;
- };
- } // namespace sot
-} // namespace dynamicgraph
+namespace sot {
+/// Define the type of input expected by the robot
+enum ControlInput {
+ CONTROL_INPUT_NO_INTEGRATION = 0,
+ CONTROL_INPUT_ONE_INTEGRATION = 1,
+ CONTROL_INPUT_TWO_INTEGRATION = 2,
+ CONTROL_INPUT_SIZE = 3
+};
+
+const std::string ControlInput_s[] = {"noInteg", "oneInteg", "twoInteg"};
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOT_CORE_EXPORT Device : public Entity {
+public:
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+
+ enum ForceSignalSource {
+ FORCE_SIGNAL_RLEG,
+ FORCE_SIGNAL_LLEG,
+ FORCE_SIGNAL_RARM,
+ FORCE_SIGNAL_LARM
+ };
+
+protected:
+ dg::Vector state_;
+ dg::Vector velocity_;
+ bool sanityCheck_;
+ dg::Vector vel_control_;
+ ControlInput controlInputType_;
+ bool withForceSignals[4];
+ PeriodicCall periodicCallBefore_;
+ PeriodicCall periodicCallAfter_;
+ double timestep_;
+
+ /// \name Robot bounds used for sanity checks
+ /// \{
+ Vector upperPosition_;
+ Vector upperVelocity_;
+ Vector upperTorque_;
+ Vector lowerPosition_;
+ Vector lowerVelocity_;
+ Vector lowerTorque_;
+ /// \}
+public:
+ /* --- CONSTRUCTION --- */
+ Device(const std::string &name);
+ /* --- DESTRUCTION --- */
+ virtual ~Device();
+
+ virtual void setStateSize(const unsigned int &size);
+ virtual void setState(const dg::Vector &st);
+ void setVelocitySize(const unsigned int &size);
+ virtual void setVelocity(const dg::Vector &vel);
+ virtual void setSecondOrderIntegration();
+ virtual void setNoIntegration();
+ virtual void setControlInputType(const std::string &cit);
+ virtual void increment(const double &dt = 5e-2);
+
+ /// \name Sanity check parameterization
+ /// \{
+ void setSanityCheck(const bool &enableCheck);
+ void setPositionBounds(const Vector &lower, const Vector &upper);
+ void setVelocityBounds(const Vector &lower, const Vector &upper);
+ void setTorqueBounds(const Vector &lower, const Vector &upper);
+ /// \}
+
+public: /* --- DISPLAY --- */
+ virtual void display(std::ostream &os) const;
+ virtual void cmdDisplay();
+ SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const Device &r) {
+ r.display(os);
+ return os;
+ }
+
+public: /* --- SIGNALS --- */
+ dynamicgraph::SignalPtr<dg::Vector, int> controlSIN;
+ dynamicgraph::SignalPtr<dg::Vector, int> attitudeSIN;
+ dynamicgraph::SignalPtr<dg::Vector, int> zmpSIN;
+
+ /// \name Device current state.
+ /// \{
+ dynamicgraph::Signal<dg::Vector, int> stateSOUT;
+ dynamicgraph::Signal<dg::Vector, int> velocitySOUT;
+ dynamicgraph::Signal<MatrixRotation, int> attitudeSOUT;
+ /*! \brief The current state of the robot from the command viewpoint. */
+ dynamicgraph::Signal<dg::Vector, int> motorcontrolSOUT;
+ dynamicgraph::Signal<dg::Vector, int> previousControlSOUT;
+ /*! \brief The ZMP reference send by the previous controller. */
+ dynamicgraph::Signal<dg::Vector, int> ZMPPreviousControllerSOUT;
+ /// \}
+
+ /// \name Real robot current state
+ /// This corresponds to the real encoders values and take into
+ /// account the stabilization step. Therefore, this usually
+ /// does *not* match the state control input signal.
+ /// \{
+ /// Motor positions
+ dynamicgraph::Signal<dg::Vector, int> robotState_;
+ /// Motor velocities
+ dynamicgraph::Signal<dg::Vector, int> robotVelocity_;
+ /// The force torque sensors
+ dynamicgraph::Signal<dg::Vector, int> *forcesSOUT[4];
+ /// Motor torques
+ /// \todo why pseudo ?
+ dynamicgraph::Signal<dg::Vector, int> pseudoTorqueSOUT;
+ /// \}
+
+protected:
+ /// Compute roll pitch yaw angles of freeflyer joint.
+ void integrateRollPitchYaw(dg::Vector &state, const dg::Vector &control,
+ double dt);
+ /// Store Position of free flyer joint
+ MatrixHomogeneous ffPose_;
+ /// Compute the new position, from the current control.
+ ///
+ /// When sanity checks are enabled, this checks that the control is within
+ /// bounds. There are three cases, depending on what the control is:
+ /// - position: checks that the position is within bounds,
+ /// - velocity: checks that the velocity and the future position are
+ /// within bounds,
+ /// - acceleration: checks that the acceleration, the future velocity and
+ /// position are within bounds.
+ /// \todo in order to check the acceleration, we need
+ /// pinocchio and the contact forces in order to estimate
+ /// the joint torques for the given acceleration.
+ virtual void integrate(const double &dt);
+
+protected:
+ /// Get freeflyer pose
+ const MatrixHomogeneous &freeFlyerPose() const;
+
+public:
+ virtual void setRoot(const dg::Matrix &root);
+
+ virtual void setRoot(const MatrixHomogeneous &worldMwaist);
+
+private:
+ // Intermediate variable to avoid dynamic allocation
+ dg::Vector forceZero6;
+};
+} // namespace sot
+} // namespace dynamicgraph
#endif /* #ifndef SOT_DEVICE_HH */
diff --git a/include/sot/core/event.hh b/include/sot/core/event.hh
index f915378a..445e452e 100644
--- a/include/sot/core/event.hh
+++ b/include/sot/core/event.hh
@@ -2,103 +2,92 @@
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
#ifndef __SOT_EVENT_H__
-# define __SOT_EVENT_H__
+#define __SOT_EVENT_H__
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/pool.h>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/pool.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/signal.h>
#include <sot/core/config.hh>
namespace dynamicgraph {
- namespace sot {
- /// Event
- 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"),
- conditionSIN(NULL,"Event("+name+")::input(bool)::condition"),
- lastVal_ (2) // lastVal_ should be different true and false.
- {
- checkSOUT.setFunction
- (boost::bind (&Event::check, this, _1, _2));
- signalRegistration (conditionSIN);
- signalRegistration (checkSOUT);
-
- using command::makeCommandVoid1;
- std::string docstring =
- "\n"
- " Add a signal\n";
- addCommand ("addSignal", makeCommandVoid1
- (*this, &Event::addSignal, docstring));
-
- docstring =
- "\n"
- " Get list of signals\n";
- addCommand ("list", new command::Getter<Event, std::string>
- (*this, &Event::getSignalsByName, docstring));
-
- docstring =
- "\n"
- " Triggers an event only when condition goes from False to True\n";
- addCommand ("setOnlyUp", new command::Setter<Event, bool>
- (*this, &Event::setOnlyUp, docstring));
- }
-
- ~Event () {}
-
- /// Header documentation of the python class
- virtual std::string getDocString () const
- {
- return
- "Send an event when the input changes\n\n"
- " The signal triggered is called whenever the condition is satisfied.\n";
- }
-
- void addSignal (const std::string& signal)
- {
- std::istringstream iss (signal);
- triggers.push_back(&PoolStorage::getInstance()->getSignal (iss));
- }
-
- // Returns the Python string representation of the list of signal names.
- std::string getSignalsByName () const
- {
- std::ostringstream oss;
- oss << "(";
- for (Triggers_t::const_iterator _sig = triggers.begin();
- _sig != triggers.end(); ++_sig)
- oss << '\'' << (*_sig)->getName() << "\', ";
- oss << ")";
- return oss.str();
- }
-
- void setOnlyUp (const bool& up)
- {
- onlyUp_ = up;
- }
-
- private:
- typedef SignalBase<int>* Trigger_t;
- typedef std::vector<Trigger_t> Triggers_t;
-
- bool& check (bool& ret, const int& time);
-
- Signal <bool, int> checkSOUT;
-
- Triggers_t triggers;
- SignalPtr <bool, int> conditionSIN;
-
- bool lastVal_, onlyUp_;
- };
- } // namespace sot
+namespace sot {
+/// Event
+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"),
+ conditionSIN(NULL, "Event(" + name + ")::input(bool)::condition"),
+ lastVal_(2) // lastVal_ should be different true and false.
+ {
+ checkSOUT.setFunction(boost::bind(&Event::check, this, _1, _2));
+ signalRegistration(conditionSIN);
+ signalRegistration(checkSOUT);
+
+ using command::makeCommandVoid1;
+ std::string docstring = "\n"
+ " Add a signal\n";
+ addCommand("addSignal",
+ makeCommandVoid1(*this, &Event::addSignal, docstring));
+
+ docstring = "\n"
+ " Get list of signals\n";
+ addCommand("list", new command::Getter<Event, std::string>(
+ *this, &Event::getSignalsByName, docstring));
+
+ docstring =
+ "\n"
+ " Triggers an event only when condition goes from False to True\n";
+ addCommand("setOnlyUp", new command::Setter<Event, bool>(
+ *this, &Event::setOnlyUp, docstring));
+ }
+
+ ~Event() {}
+
+ /// Header documentation of the python class
+ virtual std::string getDocString() const {
+ return "Send an event when the input changes\n\n"
+ " The signal triggered is called whenever the condition is "
+ "satisfied.\n";
+ }
+
+ void addSignal(const std::string &signal) {
+ std::istringstream iss(signal);
+ triggers.push_back(&PoolStorage::getInstance()->getSignal(iss));
+ }
+
+ // Returns the Python string representation of the list of signal names.
+ std::string getSignalsByName() const {
+ std::ostringstream oss;
+ oss << "(";
+ for (Triggers_t::const_iterator _sig = triggers.begin();
+ _sig != triggers.end(); ++_sig)
+ oss << '\'' << (*_sig)->getName() << "\', ";
+ oss << ")";
+ return oss.str();
+ }
+
+ void setOnlyUp(const bool &up) { onlyUp_ = up; }
+
+private:
+ typedef SignalBase<int> *Trigger_t;
+ typedef std::vector<Trigger_t> Triggers_t;
+
+ bool &check(bool &ret, const int &time);
+
+ Signal<bool, int> checkSOUT;
+
+ Triggers_t triggers;
+ SignalPtr<bool, int> conditionSIN;
+
+ bool lastVal_, onlyUp_;
+};
+} // 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 37db40a5..ec1de2cb 100644
--- a/include/sot/core/exception-abstract.hh
+++ b/include/sot/core/exception-abstract.hh
@@ -14,13 +14,11 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/* Classes standards. */
-#include <ostream> /* Classe ostream. */
-#include <string> /* Classe string. */
#include "sot/core/api.hh"
#include <exception>
-
+#include <ostream> /* Classe ostream. */
+#include <string> /* Classe string. */
// Uncomment this macros to have lines parameter on the throw display
// #define SOT_EXCEPTION_PASSING_PARAM
@@ -29,32 +27,32 @@
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* \class ExceptionAbstract
*/
-class SOT_CORE_EXPORT ExceptionAbstract : public std::exception
-{
-
- public:
-
- enum ExceptionEnum
- {
- ABSTRACT = 0
- ,SIGNAL = 100
- ,TASK = 200
- ,FEATURE = 300
- ,FACTORY = 400
- ,DYNAMIC = 500
- ,TRACES = 600
- ,TOOLS = 700
- ,PATTERN_GENERATOR= 800
- };
+class SOT_CORE_EXPORT ExceptionAbstract : public std::exception {
+
+public:
+ enum ExceptionEnum {
+ ABSTRACT = 0,
+ SIGNAL = 100,
+ TASK = 200,
+ FEATURE = 300,
+ FACTORY = 400,
+ DYNAMIC = 500,
+ TRACES = 600,
+ TOOLS = 700,
+ PATTERN_GENERATOR = 800
+ };
static const std::string EXCEPTION_NAME;
- virtual const std::string& getExceptionName( void ) const { return EXCEPTION_NAME; }
+ virtual const std::string &getExceptionName(void) const {
+ return EXCEPTION_NAME;
+ }
- protected:
+protected:
/** Error code.
* \sa ErrorCodeEnum */
int code;
@@ -63,74 +61,76 @@ class SOT_CORE_EXPORT ExceptionAbstract : public std::exception
std::string message;
private:
-
/** forbid the empty constructor (private). */
- ExceptionAbstract( void );
-public:
+ ExceptionAbstract(void);
- ExceptionAbstract( const int& code, const std::string & msg = "" );
- virtual ~ExceptionAbstract( void ) throw() {}
+public:
+ ExceptionAbstract(const int &code, const std::string &msg = "");
+ virtual ~ExceptionAbstract(void) throw() {}
/** Access to the error code. */
- int getCode (void);
+ int getCode(void);
/** Reference access to the error message (can be empty). */
- const std::string &getStringMessage (void);
+ const std::string &getStringMessage(void);
- /** Access to the pointer on the array of \e char related to the error string.
- * Cannot be \e NULL.
+ /** Access to the pointer on the array of \e char related to the error
+ * string. Cannot be \e NULL.
*/
- const char *getMessage (void);
- const char* what () const throw ();
-
+ const char *getMessage(void);
+ const char *what() const throw();
/** Print the error structure. */
- SOT_CORE_EXPORT friend std::ostream & operator << (std::ostream & os,
- const ExceptionAbstract & err);
+ SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const ExceptionAbstract &err);
#ifdef SOT_EXCEPTION_PASSING_PARAM
- public:
- class Param
- {
- public:
- static const int BUFFER_SIZE = 80;
-
- const char * functionPTR;
- char function[ BUFFER_SIZE ];
- int line;
- const char * filePTR;
- char file[ BUFFER_SIZE ];
- bool pointersSet,set;
- public:
- Param( const int& _line, const char * _function, const char * _file );
- Param( void ) : pointersSet(false),set(false) {}
- Param& initCopy( const Param& p );
-
- };
-
- protected:
+public:
+ class Param {
+ public:
+ static const int BUFFER_SIZE = 80;
+
+ const char *functionPTR;
+ char function[BUFFER_SIZE];
+ int line;
+ const char *filePTR;
+ char file[BUFFER_SIZE];
+ bool pointersSet, set;
+
+ public:
+ Param(const int &_line, const char *_function, const char *_file);
+ Param(void) : pointersSet(false), set(false) {}
+ Param &initCopy(const Param &p);
+ };
+
+protected:
mutable Param p;
- template<class Exc>
- friend const Exc& operator+ ( const ExceptionAbstract::Param& p, const Exc& e )
- { e.p.initCopy(p); return e; }
- template<class Exc>
- friend Exc& operator+ ( const ExceptionAbstract::Param& p, Exc& e )
- { e.p.initCopy(p); return e; }
+ template <class Exc>
+ friend const Exc &operator+(const ExceptionAbstract::Param &p, const Exc &e) {
+ e.p.initCopy(p);
+ return e;
+ }
+ template <class Exc>
+ friend Exc &operator+(const ExceptionAbstract::Param &p, Exc &e) {
+ e.p.initCopy(p);
+ return e;
+ }
#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
};
-#define SOT_RETHROW ( const ExceptionAbstract& err ) { throw err; }
-
-
+#define SOT_RETHROW \
+ (const ExceptionAbstract &err) { throw err; }
#ifdef SOT_EXCEPTION_PASSING_PARAM
-# define SOT_THROW throw ExceptionAbstract::Param(__LINE__,__FUNCTION__,__FILE__) +
+#define SOT_THROW \
+ throw ExceptionAbstract::Param(__LINE__, __FUNCTION__, __FILE__) +
#else //#ifdef SOT_EXCEPTION_PASSING_PARAM
-# define SOT_THROW throw
+#define SOT_THROW throw
#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_ABSTRACT_EXCEPTION_H */
diff --git a/include/sot/core/exception-dynamic.hh b/include/sot/core/exception-dynamic.hh
index 367a4476..58d78fd6 100644
--- a/include/sot/core/exception-dynamic.hh
+++ b/include/sot/core/exception-dynamic.hh
@@ -14,50 +14,47 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#include <sot/core/exception-abstract.hh>
#include "sot/core/api.hh"
+#include <sot/core/exception-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* \class ExceptionDynamic
*/
-class SOT_CORE_EXPORT ExceptionDynamic
-:public ExceptionAbstract
+class SOT_CORE_EXPORT ExceptionDynamic : public ExceptionAbstract
{
- public:
- enum ErrorCodeEnum
- {
- GENERIC = ExceptionAbstract::DYNAMIC
+public:
+ enum ErrorCodeEnum {
+ GENERIC = ExceptionAbstract::DYNAMIC
- ,CANT_DESTROY_SIGNAL
- ,JOINT_RANK
- ,DYNAMIC_JRL
- ,JOINT_SIZE
- ,INTEGRATION
- };
+ ,
+ CANT_DESTROY_SIGNAL,
+ JOINT_RANK,
+ DYNAMIC_JRL,
+ JOINT_SIZE,
+ INTEGRATION
+ };
static const std::string EXCEPTION_NAME;
- virtual const std::string& getExceptionName( void ) const { return EXCEPTION_NAME; }
+ virtual const std::string &getExceptionName(void) const {
+ return EXCEPTION_NAME;
+ }
public:
-
- ExceptionDynamic ( const ExceptionDynamic::ErrorCodeEnum& errcode,
- const std::string & msg = "" );
- ExceptionDynamic( const ExceptionDynamic::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... );
- virtual ~ExceptionDynamic( void ) throw() {}
-
-
+ ExceptionDynamic(const ExceptionDynamic::ErrorCodeEnum &errcode,
+ const std::string &msg = "");
+ ExceptionDynamic(const ExceptionDynamic::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...);
+ virtual ~ExceptionDynamic(void) throw() {}
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_DYNAMIC_EXCEPTION_H */
diff --git a/include/sot/core/exception-factory.hh b/include/sot/core/exception-factory.hh
index bef6f082..e863bbef 100644
--- a/include/sot/core/exception-factory.hh
+++ b/include/sot/core/exception-factory.hh
@@ -14,51 +14,50 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#include <sot/core/exception-abstract.hh>
#include "sot/core/api.hh"
+#include <sot/core/exception-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* \class ExceptionFactory
*/
-class SOT_CORE_EXPORT ExceptionFactory
-:public ExceptionAbstract
+class SOT_CORE_EXPORT ExceptionFactory : public ExceptionAbstract
{
public:
-
- enum ErrorCodeEnum
- {
- GENERIC = ExceptionAbstract::FACTORY
- ,UNREFERED_OBJECT
- ,UNREFERED_SIGNAL
- ,UNREFERED_FUNCTION
- ,DYNAMIC_LOADING
- ,SIGNAL_CONFLICT
- ,FUNCTION_CONFLICT
- ,OBJECT_CONFLICT
- ,SYNTAX_ERROR // j' aime bien FATAL_ERROR aussi faut que je la case qq part...
- ,READ_FILE
- };
+ enum ErrorCodeEnum {
+ GENERIC = ExceptionAbstract::FACTORY,
+ UNREFERED_OBJECT,
+ UNREFERED_SIGNAL,
+ UNREFERED_FUNCTION,
+ DYNAMIC_LOADING,
+ SIGNAL_CONFLICT,
+ FUNCTION_CONFLICT,
+ OBJECT_CONFLICT,
+ SYNTAX_ERROR // j' aime bien FATAL_ERROR aussi faut que je la case qq
+ // part...
+ ,
+ READ_FILE
+ };
static const std::string EXCEPTION_NAME;
- virtual const std::string& getExceptionName( void )const{ return ExceptionFactory::EXCEPTION_NAME; }
-
- ExceptionFactory ( const ExceptionFactory::ErrorCodeEnum& errcode,
- const std::string & msg = "" );
- ExceptionFactory ( const ExceptionFactory::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... );
- virtual ~ExceptionFactory( void ) throw() {}
-
+ virtual const std::string &getExceptionName(void) const {
+ return ExceptionFactory::EXCEPTION_NAME;
+ }
+
+ ExceptionFactory(const ExceptionFactory::ErrorCodeEnum &errcode,
+ const std::string &msg = "");
+ ExceptionFactory(const ExceptionFactory::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...);
+ virtual ~ExceptionFactory(void) throw() {}
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_EXCEPTION_FACTORY_H */
diff --git a/include/sot/core/exception-feature.hh b/include/sot/core/exception-feature.hh
index 5d1eb005..02ae7688 100644
--- a/include/sot/core/exception-feature.hh
+++ b/include/sot/core/exception-feature.hh
@@ -14,45 +14,43 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#include <sot/core/exception-abstract.hh>
#include "sot/core/api.hh"
+#include <sot/core/exception-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* \class ExceptionFeature
*/
-class SOT_CORE_EXPORT ExceptionFeature
-:public ExceptionAbstract
+class SOT_CORE_EXPORT ExceptionFeature : public ExceptionAbstract
{
public:
-
- enum ErrorCodeEnum
- {
- GENERIC = ExceptionAbstract::FEATURE
- ,BAD_INIT
- ,UNCOMPATIBLE_SIZE
- };
+ enum ErrorCodeEnum {
+ GENERIC = ExceptionAbstract::FEATURE,
+ BAD_INIT,
+ UNCOMPATIBLE_SIZE
+ };
static const std::string EXCEPTION_NAME;
- virtual const std::string& getExceptionName( void ) const { return ExceptionFeature::EXCEPTION_NAME; }
+ virtual const std::string &getExceptionName(void) const {
+ return ExceptionFeature::EXCEPTION_NAME;
+ }
- ExceptionFeature ( const ExceptionFeature::ErrorCodeEnum& errcode,
- const std::string & msg = "" );
+ ExceptionFeature(const ExceptionFeature::ErrorCodeEnum &errcode,
+ const std::string &msg = "");
- ExceptionFeature ( const ExceptionFeature::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... );
+ ExceptionFeature(const ExceptionFeature::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...);
- virtual ~ExceptionFeature( void ) throw() {}
+ virtual ~ExceptionFeature(void) throw() {}
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_EXCEPTION_FEATURE_H */
diff --git a/include/sot/core/exception-signal.hh b/include/sot/core/exception-signal.hh
index acc7b7f4..800a8bda 100644
--- a/include/sot/core/exception-signal.hh
+++ b/include/sot/core/exception-signal.hh
@@ -14,51 +14,48 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#include <sot/core/exception-abstract.hh>
#include "sot/core/api.hh"
+#include <sot/core/exception-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* \class ExceptionSignal
*/
-class SOT_CORE_EXPORT ExceptionSignal
-:public ExceptionAbstract
+class SOT_CORE_EXPORT ExceptionSignal : public ExceptionAbstract
{
- public:
- enum ErrorCodeEnum
- {
- GENERIC = ExceptionAbstract::SIGNAL
-
- ,READWRITE_LOCK
- ,COPY_NOT_INITIALIZED
- ,NOT_INITIALIZED
- ,PLUG_IMPOSSIBLE
- ,SET_IMPOSSIBLE
- ,BAD_CAST
- };
+public:
+ enum ErrorCodeEnum {
+ GENERIC = ExceptionAbstract::SIGNAL
+
+ ,
+ READWRITE_LOCK,
+ COPY_NOT_INITIALIZED,
+ NOT_INITIALIZED,
+ PLUG_IMPOSSIBLE,
+ SET_IMPOSSIBLE,
+ BAD_CAST
+ };
static const std::string EXCEPTION_NAME;
- virtual const std::string& getExceptionName( void ) const { return EXCEPTION_NAME; }
+ virtual const std::string &getExceptionName(void) const {
+ return EXCEPTION_NAME;
+ }
public:
-
- ExceptionSignal ( const ExceptionSignal::ErrorCodeEnum& errcode,
- const std::string & msg = "" );
- ExceptionSignal( const ExceptionSignal::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... );
- virtual ~ExceptionSignal( void ) throw() {}
-
-
+ ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode,
+ const std::string &msg = "");
+ ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...);
+ virtual ~ExceptionSignal(void) throw() {}
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SIGNAL_EXCEPTION_H */
diff --git a/include/sot/core/exception-task.hh b/include/sot/core/exception-task.hh
index ab12272e..e8f9a0be 100644
--- a/include/sot/core/exception-task.hh
+++ b/include/sot/core/exception-task.hh
@@ -14,47 +14,44 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#include <sot/core/exception-abstract.hh>
#include "sot/core/api.hh"
+#include <sot/core/exception-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* \class ExceptionTask
*/
-class SOT_CORE_EXPORT ExceptionTask
-:public ExceptionAbstract
+class SOT_CORE_EXPORT ExceptionTask : public ExceptionAbstract
{
public:
-
- enum ErrorCodeEnum
- {
- GENERIC = ExceptionAbstract::TASK
- ,EMPTY_LIST
- ,NON_ADEQUATE_FEATURES
- ,MATRIX_SIZE
- ,BOUND_TYPE
- ,PARSER_MULTI_BOUND
- };
+ enum ErrorCodeEnum {
+ GENERIC = ExceptionAbstract::TASK,
+ EMPTY_LIST,
+ NON_ADEQUATE_FEATURES,
+ MATRIX_SIZE,
+ BOUND_TYPE,
+ PARSER_MULTI_BOUND
+ };
static const std::string EXCEPTION_NAME;
- virtual const std::string& getExceptionName( void ) const { return EXCEPTION_NAME; }
-
- ExceptionTask ( const ExceptionTask::ErrorCodeEnum& errcode,
- const std::string & msg = "" );
- ExceptionTask( const ExceptionTask::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... );
- virtual ~ExceptionTask( void ) throw() {}
-
+ virtual const std::string &getExceptionName(void) const {
+ return EXCEPTION_NAME;
+ }
+
+ ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode,
+ const std::string &msg = "");
+ ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...);
+ virtual ~ExceptionTask(void) throw() {}
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_EXCEPTION_TASK_H */
diff --git a/include/sot/core/exception-tools.hh b/include/sot/core/exception-tools.hh
index a4fad5b2..af194872 100644
--- a/include/sot/core/exception-tools.hh
+++ b/include/sot/core/exception-tools.hh
@@ -14,52 +14,43 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#include <sot/core/exception-abstract.hh>
#include "sot/core/api.hh"
+#include <sot/core/exception-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
-
- /* \class ExceptionTools
- */
- class SOT_CORE_EXPORT ExceptionTools
- :public ExceptionAbstract
-
- {
- public:
- enum ErrorCodeEnum
- {
- GENERIC = ExceptionAbstract::TOOLS
-
- ,CORBA
- ,KALMAN_SIZE
- };
-
- static const std::string EXCEPTION_NAME;
- virtual const std::string& getExceptionName() const {
- return EXCEPTION_NAME;
- }
+namespace sot {
- public:
-
- ExceptionTools ( const ExceptionTools::ErrorCodeEnum& errcode,
- const std::string & msg = "" );
- ExceptionTools( const ExceptionTools::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... );
- virtual ~ExceptionTools( void ) throw() {}
-
-
- };
-
- } // namespace sot
+/* \class ExceptionTools
+ */
+class SOT_CORE_EXPORT ExceptionTools : public ExceptionAbstract
+
+{
+public:
+ enum ErrorCodeEnum {
+ GENERIC = ExceptionAbstract::TOOLS
+
+ ,
+ CORBA,
+ KALMAN_SIZE
+ };
+
+ static const std::string EXCEPTION_NAME;
+ virtual const std::string &getExceptionName() const { return EXCEPTION_NAME; }
+
+public:
+ ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode,
+ const std::string &msg = "");
+ ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...);
+ virtual ~ExceptionTools(void) throw() {}
+};
+
+} // 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 d796cfb3..6857cc11 100644
--- a/include/sot/core/exp-moving-avg.hh
+++ b/include/sot/core/exp-moving-avg.hh
@@ -13,15 +13,15 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include <sot/core/config.hh>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
+#include <sot/core/config.hh>
namespace dg = ::dynamicgraph;
namespace dynamicgraph {
- namespace sot {
+namespace sot {
/* --------------------------------------------------------------------- */
/* --- TRACER ---------------------------------------------------------- */
@@ -31,34 +31,30 @@ using dynamicgraph::Entity;
using dynamicgraph::SignalPtr;
using dynamicgraph::SignalTimeDependent;
-class SOT_CORE_DLLAPI ExpMovingAvg
-: public Entity
-{
+class SOT_CORE_DLLAPI ExpMovingAvg : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- SignalPtr< dg::Vector,int > updateSIN;
- SignalTimeDependent<int,int> refresherSINTERN;
- SignalTimeDependent<dg::Vector,int> averageSOUT;
+public:
+ SignalPtr<dg::Vector, int> updateSIN;
+ SignalTimeDependent<int, int> refresherSINTERN;
+ SignalTimeDependent<dg::Vector, int> averageSOUT;
- public:
- ExpMovingAvg( const std::string& n );
- virtual ~ExpMovingAvg( void );
+public:
+ ExpMovingAvg(const std::string &n);
+ virtual ~ExpMovingAvg(void);
- void setAlpha(const double& alpha_);
+ void setAlpha(const double &alpha_);
- protected:
-
- dg::Vector& update(dg::Vector& res, const int& inTime);
+protected:
+ dg::Vector &update(dg::Vector &res, const int &inTime);
dg::Vector average;
double alpha;
bool init;
-
};
- } /* namespace sot */
+} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TRACER_H__ */
diff --git a/include/sot/core/factory.hh b/include/sot/core/factory.hh
index 5e0cd417..89e9edcb 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) \
- DYNAMICGRAPH_FACTORY_ENTITY_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) \
- DYNAMICGRAPH_FACTORY_ENTITY_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 73096f70..9eef4655 100644
--- a/include/sot/core/feature-1d.hh
+++ b/include/sot/core/feature-1d.hh
@@ -15,28 +15,29 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/feature-abstract.hh>
#include <sot/core/exception-task.hh>
+#include <sot/core/feature-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_1d_EXPORTS)
-# define SOTFEATURE1D_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATURE1D_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_1d_EXPORTS)
+#define SOTFEATURE1D_EXPORT __declspec(dllexport)
#else
-# define SOTFEATURE1D_EXPORT
+#define SOTFEATURE1D_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTFEATURE1D_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
\class Feature1D
@@ -45,20 +46,18 @@ namespace dg = dynamicgraph;
of the mother task.
*/
-class SOTFEATURE1D_EXPORT Feature1D
- : public FeatureAbstract, FeatureReferenceHelper<Feature1D>
-{
+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:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+protected:
/* --- SIGNALS ------------------------------------------------------------ */
- public:
+public:
/*! \name Signals
@{
*/
@@ -66,58 +65,59 @@ class SOTFEATURE1D_EXPORT Feature1D
@{
*/
/*! \brief Input for the error. */
- dg::SignalPtr< dg::Vector,int > errorSIN;
+ dg::SignalPtr<dg::Vector, int> errorSIN;
/*! \brief Input for the Jacobian. */
- dg::SignalPtr< dg::Matrix,int > jacobianSIN;
+ dg::SignalPtr<dg::Matrix, int> jacobianSIN;
/*! @} */
/*! \name Output signals
@{
*/
- /*! \brief Publish the jacobian of the feature according to the robot state. */
+ /*! \brief Publish the jacobian of the feature according to the robot state.
+ */
using FeatureAbstract::jacobianSOUT;
- /*! \brief Publish the error between the desired and the current value of the feature. */
+ /*! \brief Publish the error between the desired and the current value of the
+ * feature. */
using FeatureAbstract::errorSOUT;
- public:
-
+public:
/*! \brief Default constructor */
- Feature1D( const std::string& name );
+ Feature1D(const std::string &name);
/*! \brief Default destructor */
- virtual ~Feature1D( void ) {}
+ virtual ~Feature1D(void) {}
/*! \brief Get the dimension of the feature. */
- virtual unsigned int& getDimension( unsigned int & dim, int time );
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
/*! \name Methods to trigger computation related to this feature.
@{
*/
- /*! \brief Compute the error between the desired value and the value itself. */
- virtual dg::Vector& computeError( dg::Vector& res,int time );
+ /*! \brief Compute the error between the desired value and the value itself.
+ */
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
/*! \brief Compute the Jacobian of the value according to the robot state.. */
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
/*! @} */
/*! \brief Display the information related to this 1D implementation. */
- virtual void display( std::ostream& os ) const;
-
+ virtual void display(std::ostream &os) const;
/*! \name Dealing with the reference value to be reach with this feature.
@{
*/
DECLARE_REFERENCE_FUNCTIONS(Feature1D);
/*! @} */
+};
-} ;
-
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_1D_HH__
diff --git a/include/sot/core/feature-abstract.hh b/include/sot/core/feature-abstract.hh
index 7caa6321..5ee14cfb 100644
--- a/include/sot/core/feature-abstract.hh
+++ b/include/sot/core/feature-abstract.hh
@@ -19,276 +19,269 @@
namespace dg = dynamicgraph;
/* SOT */
-#include <sot/core/flags.hh>
+#include "sot/core/api.hh"
+#include "sot/core/deprecated.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"
-#include "sot/core/deprecated.hh"
/* STD */
#include <string>
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- /*! \class FeatureAbstract
- \ingroup features
- \brief This class gives the abstract definition of a feature.
-
- \par par_features_definition Definition
- In short, a feature is a data evolving according to time. It is defined
- by a vector \f${\bf s}({\bf q}) \in \mathbb{R}^n \f$ where \f$ {\bf q} \f$ is a robot
- configuration, which depends on the time \f$ t \f$.
- By default a feature has a desired \f${\bf s}^*(t) \f$.
- \f${\bf s}^*\f$ is provided by another feature of the same type called
- reference. The
- feature is in charge of collecting its own current state. A feature is
- supposed to compute an error between its current state and the desired
- one: \f$ E(t) = e({\bf q}(t), t) = {\bf s}({\bf q}(t)) \ominus {\bf s}^*(t) \f$.
- Here, \f$ \ominus \f$ is the difference operator of Lie group in which
- \f$ {\bf s} \f$ and \f$ {\bf s}^* \f$ are. The documentation below assumes
- the Lie group is a vector space and \f$\ominus\f$ is the usual difference
- operator.
-
- A feature computes:
- \li the Jacobian according to the robot state vector \f$ J =
- \frac{\partial e}{\partial {\bf q}} = \frac{\partial{\bf s}}{\partial {\bf q}}\f$.
- \li the partial derivative of the error \f$ e \f$:
- \f$ \frac{\partial e}{\partial t} = - \frac{d{\bf s}^*}{dt}\f$.
-
- The task is in general computed from the value of the feature at the
- current instant \f$E(t) = e({\bf q},t)\f$. The derivative of \f$ E \f$ is:
- \f[
- \frac{dE}{dt} = J({\bf q}) \dot{q} + \frac{\partial e}{\partial t}
- \f]
-
- \image html feature.png "Feature diagram: Feature types derive from
- FeatureAbstract. Each feature has a reference of the same type and
- compute an error by comparing
- errorSIN
- signals from itself and from the
- reference."
-
- */
- class SOT_CORE_EXPORT FeatureAbstract
- :public Entity
- {
- public:
- /*! \brief Store the name of the class. */
- static const std::string CLASS_NAME;
-
- /*! \brief Returns the name class. */
- virtual const std::string& getClassName( void ) const
- { return CLASS_NAME; }
-
- /*! \brief Register the feature in the stack of tasks. */
- void featureRegistration( void );
-
- void initCommands( void );
-
- public:
- /*! \brief Default constructor: the name of the class should be given. */
- FeatureAbstract( const std::string& name );
- /*! \brief Default destructor */
- virtual ~FeatureAbstract( void ) {};
-
- /*! \name Methods returning the dimension of the feature.
- @{ */
-
- /*! \brief Verbose method.
- \par res: The integer in which the dimension will be return.
- \par time: The time at which the feature should be considered.
- \return Dimension of the feature.
- \note Be careful with features changing their dimension according to time.
- */
- virtual unsigned int& getDimension( unsigned int& res,int time ) = 0;
-
- /*! \brief Short method
- \par time: The time at which the feature should be considered.
- \return Dimension of the feature.
- \note Be careful with features changing their dimension according to time.
- */
- inline unsigned int getDimension( int time )
- {unsigned int res; getDimension(res,time); return res;}
-
- /*! \brief Shortest method
- \return Dimension of the feature.
- \note The feature is not changing its dimension according to time.
- */
- inline unsigned int getDimension( void ) const
- { return dimensionSOUT.accessCopy(); }
- /*! @} */
-
- /*! \name Methods to control internal computation.
- The main idea is that some feature may have a lower frequency
- than the internal control loop. In this case, the methods for
- computation are called only when needed.
-
- @{*/
-
- /*! \brief Compute the error between the desired feature and
- the current value of the feature measured or deduced from the robot state.
-
- \par[out] res: The error will be set into res.
- \par[in] time: The time at which the error is computed.
- \return The vector res with the appropriate value.
- */
- virtual dg::Vector& computeError( dg::Vector& res,int time ) = 0;
-
- /*! \brief Compute the Jacobian of the error according the robot state.
-
- \par[out] res: The matrix in which the error will be written.
- \return The matrix res with the appropriate values.
- */
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time ) = 0;
-
- /// Callback for signal errordotSOUT
- ///
- /// Copy components of the input signal errordotSIN defined by selection
- /// flag selectionSIN.
- virtual dg::Vector& computeErrorDot (dg::Vector& res,int time);
-
- /*! @} */
-
- /* --- SIGNALS ------------------------------------------------------------ */
- public:
-
- /*! \name Signals
- @{
- */
-
- /*! \name Input signals:
- @{ */
- /*! \brief This vector specifies which dimension are used to perform the computation.
- For instance let us assume that the feature is a 3D point. If only the Y-axis should
- be used for computing error, activation and Jacobian,
- then the vector to specify
- is \f$ [ 0 1 0] \f$.*/
- SignalPtr< Flags,int > selectionSIN;
-
- /// Derivative of the reference value.
- SignalPtr< dg::Vector,int > errordotSIN;
-
- /*! @} */
-
- /*! \name Output signals:
- @{ */
-
- /*! \brief This signal returns the error between the desired value and
- the current value : \f$ E(t) = {\bf s}(t) - {\bf s}^*(t)\f$ */
- SignalTimeDependent<dg::Vector,int> errorSOUT;
-
- /*! \brief Derivative of the error with respect to time:
- * \f$ \frac{\partial e}{\partial t} = - \frac{d{\bf s}^*}{dt} \f$ */
- SignalTimeDependent< dg::Vector,int > errordotSOUT;
-
- /*! \brief Jacobian of the error wrt the robot state:
- * \f$ J = \frac{\partial {\bf s}}{\partial {\bf q}}\f$ */
- SignalTimeDependent<dg::Matrix,int> jacobianSOUT;
-
- /*! \brief Returns the dimension of the feature as an output signal. */
- SignalTimeDependent<unsigned int,int> dimensionSOUT;
-
- /*! \brief This method write a graph description on the file named
- FileName. */
- virtual std::ostream & writeGraph(std::ostream & os) const;
-
- virtual SignalTimeDependent<dg::Vector,int>& getErrorDot()
- {
- return errordotSOUT;
- }
-
- /*! @} */
-
- /* --- REFERENCE VALUE S* ------------------------------------------------- */
- public:
-
- /*! \name Reference
- @{
- */
- virtual void setReference( FeatureAbstract * sdes ) = 0;
- virtual void unsetReference( void ) { setReference(NULL); }
- virtual const FeatureAbstract * getReferenceAbstract( void ) const = 0;
- virtual FeatureAbstract * getReferenceAbstract( void ) = 0;
- virtual bool isReferenceSet( void ) const { return false; }
-
- virtual void addDependenciesFromReference( void ) = 0;
- virtual void removeDependenciesFromReference( void ) = 0;
-
- /* Commands for bindings. */
- void setReferenceByName( const std::string& name );
- std::string getReferenceByName( void ) const ;
- /*! @} */
- };
-
-
- template <class FeatureSpecialized>
- class FeatureReferenceHelper
- {
- FeatureSpecialized * ptr;
- FeatureAbstract * ptrA;
-
- public:
- FeatureReferenceHelper( void ) : ptr (NULL) {}
-
- void setReference( FeatureAbstract * sdes );
- //void setReferenceByName( const std::string & name );
- void unsetReference( void ) { setReference(NULL); }
- bool isReferenceSet( void ) const { return ptr != NULL; }
- FeatureSpecialized * getReference( void ){ return ptr; }
- const FeatureSpecialized * getReference( void ) const { return ptr; }
- };
-
-
- template <class FeatureSpecialized>
- void FeatureReferenceHelper<FeatureSpecialized>::
- setReference( FeatureAbstract * sdes )
- {
- ptr = dynamic_cast<FeatureSpecialized*> (sdes);
- 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 ); \
- 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 ) {} \
- /* To force a ; */bool NO_REFERENCE
- /* END OF define DECLARE_REFERENCE_FUNCTIONS */
-
- } // namespace sot
+namespace sot {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/*! \class FeatureAbstract
+ \ingroup features
+ \brief This class gives the abstract definition of a feature.
+
+ \par par_features_definition Definition
+ In short, a feature is a data evolving according to time. It is defined
+ by a vector \f${\bf s}({\bf q}) \in \mathbb{R}^n \f$ where \f$ {\bf q} \f$ is
+ a robot configuration, which depends on the time \f$ t \f$. By default a
+ feature has a desired \f${\bf s}^*(t) \f$. \f${\bf s}^*\f$ is provided by
+ another feature of the same type called reference. The feature is in charge of
+ collecting its own current state. A feature is supposed to compute an error
+ between its current state and the desired one: \f$ E(t) = e({\bf q}(t), t) =
+ {\bf s}({\bf q}(t)) \ominus {\bf s}^*(t) \f$. Here, \f$ \ominus \f$ is the
+ difference operator of Lie group in which \f$ {\bf s} \f$ and \f$ {\bf s}^*
+ \f$ are. The documentation below assumes the Lie group is a vector space and
+ \f$\ominus\f$ is the usual difference operator.
+
+ A feature computes:
+ \li the Jacobian according to the robot state vector \f$ J =
+ \frac{\partial e}{\partial {\bf q}} = \frac{\partial{\bf s}}{\partial {\bf
+ q}}\f$. \li the partial derivative of the error \f$ e \f$: \f$ \frac{\partial
+ e}{\partial t} = - \frac{d{\bf s}^*}{dt}\f$.
+
+ The task is in general computed from the value of the feature at the
+ current instant \f$E(t) = e({\bf q},t)\f$. The derivative of \f$ E \f$ is:
+ \f[
+ \frac{dE}{dt} = J({\bf q}) \dot{q} + \frac{\partial e}{\partial t}
+ \f]
+
+ \image html feature.png "Feature diagram: Feature types derive from
+ FeatureAbstract. Each feature has a reference of the same type and
+ compute an error by comparing
+ errorSIN
+ signals from itself and from the
+ reference."
+
+*/
+class SOT_CORE_EXPORT FeatureAbstract : public Entity {
+public:
+ /*! \brief Store the name of the class. */
+ static const std::string CLASS_NAME;
+
+ /*! \brief Returns the name class. */
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+
+ /*! \brief Register the feature in the stack of tasks. */
+ void featureRegistration(void);
+
+ void initCommands(void);
+
+public:
+ /*! \brief Default constructor: the name of the class should be given. */
+ FeatureAbstract(const std::string &name);
+ /*! \brief Default destructor */
+ virtual ~FeatureAbstract(void){};
+
+ /*! \name Methods returning the dimension of the feature.
+ @{ */
+
+ /*! \brief Verbose method.
+ \par res: The integer in which the dimension will be return.
+ \par time: The time at which the feature should be considered.
+ \return Dimension of the feature.
+ \note Be careful with features changing their dimension according to time.
+ */
+ virtual unsigned int &getDimension(unsigned int &res, int time) = 0;
+
+ /*! \brief Short method
+ \par time: The time at which the feature should be considered.
+ \return Dimension of the feature.
+ \note Be careful with features changing their dimension according to time.
+ */
+ inline unsigned int getDimension(int time) {
+ unsigned int res;
+ getDimension(res, time);
+ return res;
+ }
+
+ /*! \brief Shortest method
+ \return Dimension of the feature.
+ \note The feature is not changing its dimension according to time.
+ */
+ inline unsigned int getDimension(void) const {
+ return dimensionSOUT.accessCopy();
+ }
+ /*! @} */
+
+ /*! \name Methods to control internal computation.
+ The main idea is that some feature may have a lower frequency
+ than the internal control loop. In this case, the methods for
+ computation are called only when needed.
+
+ @{*/
+
+ /*! \brief Compute the error between the desired feature and
+ the current value of the feature measured or deduced from the robot state.
+
+ \par[out] res: The error will be set into res.
+ \par[in] time: The time at which the error is computed.
+ \return The vector res with the appropriate value.
+ */
+ virtual dg::Vector &computeError(dg::Vector &res, int time) = 0;
+
+ /*! \brief Compute the Jacobian of the error according the robot state.
+
+ \par[out] res: The matrix in which the error will be written.
+ \return The matrix res with the appropriate values.
+ */
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time) = 0;
+
+ /// Callback for signal errordotSOUT
+ ///
+ /// Copy components of the input signal errordotSIN defined by selection
+ /// flag selectionSIN.
+ virtual dg::Vector &computeErrorDot(dg::Vector &res, int time);
+
+ /*! @} */
+
+ /* --- SIGNALS ------------------------------------------------------------ */
+public:
+ /*! \name Signals
+ @{
+ */
+
+ /*! \name Input signals:
+ @{ */
+ /*! \brief This vector specifies which dimension are used to perform the
+ computation. For instance let us assume that the feature is a 3D point. If
+ only the Y-axis should be used for computing error, activation and Jacobian,
+ then the vector to specify
+ is \f$ [ 0 1 0] \f$.*/
+ SignalPtr<Flags, int> selectionSIN;
+
+ /// Derivative of the reference value.
+ SignalPtr<dg::Vector, int> errordotSIN;
+
+ /*! @} */
+
+ /*! \name Output signals:
+ @{ */
+
+ /*! \brief This signal returns the error between the desired value and
+ the current value : \f$ E(t) = {\bf s}(t) - {\bf s}^*(t)\f$ */
+ SignalTimeDependent<dg::Vector, int> errorSOUT;
+
+ /*! \brief Derivative of the error with respect to time:
+ * \f$ \frac{\partial e}{\partial t} = - \frac{d{\bf s}^*}{dt} \f$ */
+ SignalTimeDependent<dg::Vector, int> errordotSOUT;
+
+ /*! \brief Jacobian of the error wrt the robot state:
+ * \f$ J = \frac{\partial {\bf s}}{\partial {\bf q}}\f$ */
+ SignalTimeDependent<dg::Matrix, int> jacobianSOUT;
+
+ /*! \brief Returns the dimension of the feature as an output signal. */
+ SignalTimeDependent<unsigned int, int> dimensionSOUT;
+
+ /*! \brief This method write a graph description on the file named
+ FileName. */
+ virtual std::ostream &writeGraph(std::ostream &os) const;
+
+ virtual SignalTimeDependent<dg::Vector, int> &getErrorDot() {
+ return errordotSOUT;
+ }
+
+ /*! @} */
+
+ /* --- REFERENCE VALUE S* ------------------------------------------------- */
+public:
+ /*! \name Reference
+ @{
+ */
+ virtual void setReference(FeatureAbstract *sdes) = 0;
+ virtual void unsetReference(void) { setReference(NULL); }
+ virtual const FeatureAbstract *getReferenceAbstract(void) const = 0;
+ virtual FeatureAbstract *getReferenceAbstract(void) = 0;
+ virtual bool isReferenceSet(void) const { return false; }
+
+ virtual void addDependenciesFromReference(void) = 0;
+ virtual void removeDependenciesFromReference(void) = 0;
+
+ /* Commands for bindings. */
+ void setReferenceByName(const std::string &name);
+ std::string getReferenceByName(void) const;
+ /*! @} */
+};
+
+template <class FeatureSpecialized> class FeatureReferenceHelper {
+ FeatureSpecialized *ptr;
+ FeatureAbstract *ptrA;
+
+public:
+ FeatureReferenceHelper(void) : ptr(NULL) {}
+
+ void setReference(FeatureAbstract *sdes);
+ // void setReferenceByName( const std::string & name );
+ void unsetReference(void) { setReference(NULL); }
+ bool isReferenceSet(void) const { return ptr != NULL; }
+ FeatureSpecialized *getReference(void) { return ptr; }
+ const FeatureSpecialized *getReference(void) const { return ptr; }
+};
+
+template <class FeatureSpecialized>
+void FeatureReferenceHelper<FeatureSpecialized>::setReference(
+ FeatureAbstract *sdes) {
+ ptr = dynamic_cast<FeatureSpecialized *>(sdes);
+ 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); \
+ 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) {} \
+ /* To force a ; */ bool NO_REFERENCE
+/* END OF define DECLARE_REFERENCE_FUNCTIONS */
+
+} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __SOT_FEATURE_ABSTRACT_H__
diff --git a/include/sot/core/feature-generic.hh b/include/sot/core/feature-generic.hh
index d0b20501..21bfe832 100644
--- a/include/sot/core/feature-generic.hh
+++ b/include/sot/core/feature-generic.hh
@@ -15,33 +15,35 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/feature-abstract.hh>
#include <sot/core/exception-task.hh>
+#include <sot/core/feature-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_generic_EXPORTS)
-# define SOTFEATUREGENERIC_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATUREGENERIC_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_generic_EXPORTS)
+#define SOTFEATUREGENERIC_EXPORT __declspec(dllexport)
#else
-# define SOTFEATUREGENERIC_EXPORT
+#define SOTFEATUREGENERIC_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTFEATUREGENERIC_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
\class FeatureGeneric
- \brief Class that defines a generic implementation of the abstract interface for features.
+ \brief Class that defines a generic implementation of the abstract interface
+ for features.
This class is very useful if the feature can be easily computed using
the basic operator provided. For instance a free space controller on a
@@ -53,20 +55,20 @@ namespace dg = dynamicgraph;
*/
class SOTFEATUREGENERIC_EXPORT FeatureGeneric
-: public FeatureAbstract, FeatureReferenceHelper<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; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- protected:
+protected:
dg::Vector::Index dimensionDefault;
/* --- SIGNALS ------------------------------------------------------------ */
- public:
+public:
/*! \name dg::Signals
@{
*/
@@ -74,60 +76,59 @@ class SOTFEATUREGENERIC_EXPORT FeatureGeneric
@{
*/
/*! \brief Input for the error. */
- dg::SignalPtr< dg::Vector,int > errorSIN;
+ dg::SignalPtr<dg::Vector, int> errorSIN;
/*! \brief Input for the Jacobian. */
- dg::SignalPtr< dg::Matrix,int > jacobianSIN;
+ dg::SignalPtr<dg::Matrix, int> jacobianSIN;
/*! @} */
/*! \name Output signals
@{
*/
- /*! \brief Publish the jacobian of the feature according to the robot state. */
+ /*! \brief Publish the jacobian of the feature according to the robot state.
+ */
using FeatureAbstract::jacobianSOUT;
/*! \brief Publish the error between the desired and the current value of the
feature. */
using FeatureAbstract::errorSOUT;
- public:
-
+public:
/*! \brief Default constructor */
- FeatureGeneric( const std::string& name );
+ FeatureGeneric(const std::string &name);
/*! \brief Default destructor */
- virtual ~FeatureGeneric( void ) {}
+ virtual ~FeatureGeneric(void) {}
/*! \brief Get the dimension of the feature. */
- virtual unsigned int& getDimension( unsigned int & dim, int time );
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
/*! \name Methods to trigger computation related to this feature.
@{
*/
- /*! \brief Compute the error between the desired value and the value itself. */
- virtual dg::Vector& computeError( dg::Vector& res,int time );
+ /*! \brief Compute the error between the desired value and the value itself.
+ */
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
/*! \brief Compute the Jacobian of the value according to the robot state.. */
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
/*! @} */
/*! \brief Display the information related to this generic implementation. */
- virtual void display( std::ostream& os ) const;
+ virtual void display(std::ostream &os) const;
/*! \name Dealing with the reference value to be reach with this feature.
@{
*/
DECLARE_REFERENCE_FUNCTIONS(FeatureGeneric);
/*! @} */
+};
-
-} ;
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_GENERIC_HH__
diff --git a/include/sot/core/feature-joint-limits.hh b/include/sot/core/feature-joint-limits.hh
index d630265d..fbb67c1a 100644
--- a/include/sot/core/feature-joint-limits.hh
+++ b/include/sot/core/feature-joint-limits.hh
@@ -15,28 +15,29 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/feature-abstract.hh>
#include <sot/core/exception-task.hh>
+#include <sot/core/feature-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_joint_limits_EXPORTS)
-# define SOTFEATUREJOINTLIMITS_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATUREJOINTLIMITS_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_joint_limits_EXPORTS)
+#define SOTFEATUREJOINTLIMITS_EXPORT __declspec(dllexport)
+#else
+#define SOTFEATUREJOINTLIMITS_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTFEATUREJOINTLIMITS_EXPORT
+#define SOTFEATUREJOINTLIMITS_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
@@ -44,34 +45,32 @@ namespace dg = dynamicgraph;
\brief Class that defines gradient vector for jl avoidance.
*/
class SOTFEATUREJOINTLIMITS_EXPORT FeatureJointLimits
- : public FeatureAbstract, FeatureReferenceHelper<FeatureJointLimits>
-{
+ : public FeatureAbstract,
+ FeatureReferenceHelper<FeatureJointLimits> {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+protected:
double threshold;
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; */
+ /* unsigned int freeFloatingIndex,freeFloatingSize; */
+ /* static const unsigned int FREE_FLOATING_INDEX = 0; */
+ /* static const unsigned int FREE_FLOATING_SIZE = 5; */
/* --- SIGNALS ------------------------------------------------------------ */
- public:
-
- dg::SignalPtr< dg::Vector,int > jointSIN;
- dg::SignalPtr< dg::Vector,int > upperJlSIN;
- dg::SignalPtr< dg::Vector,int > lowerJlSIN;
- dg::SignalTimeDependent< dg::Vector,int > widthJlSINTERN;
+public:
+ dg::SignalPtr<dg::Vector, int> jointSIN;
+ dg::SignalPtr<dg::Vector, int> upperJlSIN;
+ dg::SignalPtr<dg::Vector, int> lowerJlSIN;
+ dg::SignalTimeDependent<dg::Vector, int> widthJlSINTERN;
using FeatureAbstract::selectionSIN;
- using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::errorSOUT;
+ using FeatureAbstract::jacobianSOUT;
/*! \name Dealing with the reference value to be reach with this feature.
@{
@@ -79,23 +78,24 @@ class SOTFEATUREJOINTLIMITS_EXPORT FeatureJointLimits
DECLARE_REFERENCE_FUNCTIONS(FeatureJointLimits);
/*! @} */
- public:
- FeatureJointLimits( const std::string& name );
- virtual ~FeatureJointLimits( void ) {}
+public:
+ FeatureJointLimits(const std::string &name);
+ virtual ~FeatureJointLimits(void) {}
- virtual unsigned int& getDimension( unsigned int & dim, int time );
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
- virtual dg::Vector& computeError( dg::Vector& res,int time );
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
- dg::Vector& computeWidthJl( dg::Vector& res,const int& time );
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
+ dg::Vector &computeWidthJl(dg::Vector &res, const int &time);
/** Static Feature selection. */
- inline static Flags selectActuated( void );
+ inline static Flags selectActuated(void);
- virtual void display( std::ostream& os ) const;
-} ;
+ virtual void display(std::ostream &os) const;
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_JOINTLIMITS_HH__
diff --git a/include/sot/core/feature-line-distance.hh b/include/sot/core/feature-line-distance.hh
index 06876ec2..37034c4a 100644
--- a/include/sot/core/feature-line-distance.hh
+++ b/include/sot/core/feature-line-distance.hh
@@ -15,28 +15,29 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/feature-abstract.hh>
#include <sot/core/exception-task.hh>
+#include <sot/core/feature-abstract.hh>
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_line_distance_EXPORTS)
-# define SOTFEATURELINEDISTANCE_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATURELINEDISTANCE_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_line_distance_EXPORTS)
+#define SOTFEATURELINEDISTANCE_EXPORT __declspec(dllexport)
+#else
+#define SOTFEATURELINEDISTANCE_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTFEATURELINEDISTANCE_EXPORT
+#define SOTFEATURELINEDISTANCE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
@@ -44,26 +45,24 @@ namespace dg = dynamicgraph;
\brief Class that defines point-3d control feature
*/
class SOTFEATURELINEDISTANCE_EXPORT FeatureLineDistance
-: public FeatureAbstract
-{
+ : public FeatureAbstract {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+protected:
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
- dg::SignalPtr< dg::Matrix,int > articularJacobianSIN;
- dg::SignalPtr< dg::Vector,int > positionRefSIN;
- dg::SignalPtr< dg::Vector,int > vectorSIN;
- dg::SignalTimeDependent<dg::Vector,int> lineSOUT;
+public:
+ dg::SignalPtr<MatrixHomogeneous, int> positionSIN;
+ dg::SignalPtr<dg::Matrix, int> articularJacobianSIN;
+ dg::SignalPtr<dg::Vector, int> positionRefSIN;
+ dg::SignalPtr<dg::Vector, int> vectorSIN;
+ dg::SignalTimeDependent<dg::Vector, int> lineSOUT;
- using FeatureAbstract::selectionSIN;
- using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::errorSOUT;
+ using FeatureAbstract::jacobianSOUT;
+ using FeatureAbstract::selectionSIN;
/*! \name Dealing with the reference value to be reach with this feature.
@{
@@ -71,23 +70,21 @@ class SOTFEATURELINEDISTANCE_EXPORT FeatureLineDistance
DECLARE_NO_REFERENCE;
/*! @} */
+public:
+ FeatureLineDistance(const std::string &name);
+ virtual ~FeatureLineDistance(void) {}
- public:
- FeatureLineDistance( const std::string& name );
- virtual ~FeatureLineDistance( void ) {}
-
- virtual unsigned int& getDimension( unsigned int & dim, int time );
-
- virtual dg::Vector& computeError( dg::Vector& res,int time );
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
- dg::Vector& computeLineCoordinates( dg::Vector& cood,int time );
-
- virtual void display( std::ostream& os ) const;
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
-} ;
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
+ dg::Vector &computeLineCoordinates(dg::Vector &cood, int time);
+ virtual void display(std::ostream &os) const;
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_LINEDISTANCE_HH__
diff --git a/include/sot/core/feature-point6d-relative.hh b/include/sot/core/feature-point6d-relative.hh
index bcf36e92..73468ce8 100644
--- a/include/sot/core/feature-point6d-relative.hh
+++ b/include/sot/core/feature-point6d-relative.hh
@@ -15,30 +15,31 @@
/* --------------------------------------------------------------------- */
/* SOT */
+#include <sot/core/exception-task.hh>
#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-point6d.hh>
-#include <sot/core/exception-task.hh>
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_point6d_relative_EXPORTS)
-# define SOTFEATUREPOINT6DRELATIVE_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATUREPOINT6DRELATIVE_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_point6d_relative_EXPORTS)
+#define SOTFEATUREPOINT6DRELATIVE_EXPORT __declspec(dllexport)
+#else
+#define SOTFEATUREPOINT6DRELATIVE_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTFEATUREPOINT6DRELATIVE_EXPORT
+#define SOTFEATUREPOINT6DRELATIVE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
@@ -47,22 +48,19 @@ namespace dg = dynamicgraph;
point.
*/
class SOTFEATUREPOINT6DRELATIVE_EXPORT FeaturePoint6dRelative
-: public FeaturePoint6d
-{
+ : public FeaturePoint6d {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- protected:
+protected:
dg::Matrix L;
-
-
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalPtr< MatrixHomogeneous,int > positionReferenceSIN;
- dg::SignalPtr< dg::Matrix,int > articularJacobianReferenceSIN;
+public:
+ dg::SignalPtr<MatrixHomogeneous, int> positionReferenceSIN;
+ dg::SignalPtr<dg::Matrix, int> articularJacobianReferenceSIN;
/*! dg::Signals related to the computation of the derivative of
the error
@@ -71,29 +69,29 @@ class SOTFEATUREPOINT6DRELATIVE_EXPORT FeaturePoint6dRelative
/*! dg::Signals giving the derivative of the input signals.
@{*/
/*! Derivative of the relative position. */
- dg::SignalPtr< MatrixHomogeneous,int > dotpositionSIN;
+ dg::SignalPtr<MatrixHomogeneous, int> dotpositionSIN;
/*! Derivative of the reference position. */
- dg::SignalPtr< MatrixHomogeneous,int > dotpositionReferenceSIN;
+ dg::SignalPtr<MatrixHomogeneous, int> dotpositionReferenceSIN;
/*! @} */
using FeaturePoint6d::getReference;
- public:
- FeaturePoint6dRelative( const std::string& name );
- virtual ~FeaturePoint6dRelative( void ) {}
-
- virtual dg::Vector& computeError( dg::Vector& res,int time );
- virtual dg::Vector& computeErrorDot( dg::Vector& res,int time );
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+public:
+ FeaturePoint6dRelative(const std::string &name);
+ virtual ~FeaturePoint6dRelative(void) {}
- virtual void display( std::ostream& os ) const;
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
+ virtual dg::Vector &computeErrorDot(dg::Vector &res, int time);
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
- void initCommands( void );
- void initSdes( const std::string& featureDesiredName );
+ virtual void display(std::ostream &os) const;
-} ;
+ void initCommands(void);
+ void initSdes(const std::string &featureDesiredName);
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_POINT6DRELATIVE_HH__
diff --git a/include/sot/core/feature-point6d.hh b/include/sot/core/feature-point6d.hh
index bfd084e0..57da63dd 100644
--- a/include/sot/core/feature-point6d.hh
+++ b/include/sot/core/feature-point6d.hh
@@ -15,30 +15,31 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/feature-abstract.hh>
-#include <sot/core/exception-task.hh>
#include <sot/core/exception-feature.hh>
+#include <sot/core/exception-task.hh>
+#include <sot/core/feature-abstract.hh>
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_point6d_EXPORTS)
-# define SOTFEATUREPOINT6D_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATUREPOINT6D_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_point6d_EXPORTS)
+#define SOTFEATUREPOINT6D_EXPORT __declspec(dllexport)
#else
-# define SOTFEATUREPOINT6D_EXPORT
+#define SOTFEATUREPOINT6D_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTFEATUREPOINT6D_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
@@ -46,81 +47,80 @@ namespace dg = dynamicgraph;
\brief Class that defines point-6d control feature
*/
class SOTFEATUREPOINT6D_EXPORT FeaturePoint6d
- : public FeatureAbstract, public FeatureReferenceHelper<FeaturePoint6d>
-{
+ : public FeatureAbstract,
+ public FeatureReferenceHelper<FeaturePoint6d> {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
/* --- Frame type --------------------------------------------------------- */
- protected:
- enum ComputationFrameType
- {
- FRAME_DESIRED
- ,FRAME_CURRENT
- };
+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);
+ void computationFrame(const std::string &inFrame);
/// \brief Get computation frame
std::string computationFrame() const;
- private:
+
+private:
ComputationFrameType computationFrame_;
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
- dg::SignalPtr< dg::Vector, int > velocitySIN;
- dg::SignalPtr< dg::Matrix,int > articularJacobianSIN;
+public:
+ dg::SignalPtr<MatrixHomogeneous, int> positionSIN;
+ dg::SignalPtr<dg::Vector, int> velocitySIN;
+ dg::SignalPtr<dg::Matrix, int> articularJacobianSIN;
- using FeatureAbstract::selectionSIN;
- using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::errorSOUT;
+ using FeatureAbstract::jacobianSOUT;
+ using FeatureAbstract::selectionSIN;
/*! \name Dealing with the reference value to be reach with this feature.
@{ */
DECLARE_REFERENCE_FUNCTIONS(FeaturePoint6d);
/*! @} */
- public:
- FeaturePoint6d( const std::string& name );
- virtual ~FeaturePoint6d( void ) {}
+public:
+ FeaturePoint6d(const std::string &name);
+ virtual ~FeaturePoint6d(void) {}
- virtual unsigned int& getDimension( unsigned int & dim, int time );
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
- virtual dg::Vector& computeError( dg::Vector& res,int time );
- virtual dg::Vector& computeErrordot( dg::Vector& res,int time );
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
+ virtual dg::Vector &computeErrordot(dg::Vector &res, int time);
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
/** Static Feature selection. */
- inline static Flags selectX( void ) { return FLAG_LINE_1; }
- inline static Flags selectY( void ) { return FLAG_LINE_2; }
- inline static Flags selectZ( void ) { return FLAG_LINE_3; }
- inline static Flags selectRX( void ) { return FLAG_LINE_4; }
- inline static Flags selectRY( void ) { return FLAG_LINE_5; }
- inline static Flags selectRZ( void ) { return FLAG_LINE_6; }
+ inline static Flags selectX(void) { return FLAG_LINE_1; }
+ inline static Flags selectY(void) { return FLAG_LINE_2; }
+ inline static Flags selectZ(void) { return FLAG_LINE_3; }
+ inline static Flags selectRX(void) { return FLAG_LINE_4; }
+ inline static Flags selectRY(void) { return FLAG_LINE_5; }
+ inline static Flags selectRZ(void) { return FLAG_LINE_6; }
+
+ inline static Flags selectTranslation(void) { return Flags(7); }
+ inline static Flags selectRotation(void) { return Flags(56); }
- inline static Flags selectTranslation( void ) { return Flags(7); }
- inline static Flags selectRotation( void ) { return Flags(56); }
+ virtual void display(std::ostream &os) const;
- virtual void display( std::ostream& os ) const;
+public:
+ void servoCurrentPosition(void);
- 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_;
+ Eigen::Vector3d v_, omega_, errordot_t_, errordot_th_, Rreftomega_, t_, tref_;
+ VectorUTheta error_th_;
MatrixRotation R_, Rref_, Rt_, Rreft_;
Eigen::Matrix3d P_, Pinv_;
double accuracy_;
- void inverseJacobianRodrigues ();
-} ;
+ void inverseJacobianRodrigues();
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_POINT6D_HH__
diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh
index a0131e2b..5aa435a2 100644
--- a/include/sot/core/feature-pose.hh
+++ b/include/sot/core/feature-pose.hh
@@ -22,14 +22,12 @@
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/// Enum used to specify what difference operation is used in FeaturePose.
-enum Representation_t {
- SE3Representation,
- R3xSO3Representation
-};
+enum Representation_t { SE3Representation, R3xSO3Representation };
/*!
\brief Feature that controls the relative (or absolute) pose between
@@ -39,19 +37,21 @@ enum Representation_t {
- the descent direction,
- the meaning of the mask.
With R3xSO3Representation, the mask is relative to <em>Frame A</em>.
- If this feature is alone in a SOT, the relative motion of <em>Frame B</em>
- wrt <em>Frame A</em> will be a line.
- This is what most people want.
+ If this feature is alone in a SOT, the relative motion of <em>Frame
+ B</em> wrt <em>Frame A</em> will be a line. This is what most people want.
Notations:
- The frames are refered to with \c fa and \c fb.
- - Each frame is attached to a joint, which are refered to with \c ja and \c jb.
- - the difference operator is defined differently depending on the representation:
+ - Each frame is attached to a joint, which are refered to with \c ja and \c
+ jb.
+ - the difference operator is defined differently depending on the
+ representation:
- R3xSO3Representation:
\f[ \begin{array}{ccccc}
- \ominus & : & (R^3\times SO(3))^2 & \to & R^3 \times \mathfrak{so}(3) \\
- & & (a = (t_a,R_a),b = (t_b,R_b)) & \mapsto & b \ominus a = (t_b - t_a, \log(R_a^{-1} R_b) \\
- \end{array} \f]
+ \ominus & : & (R^3\times SO(3))^2 & \to & R^3 \times
+ \mathfrak{so}(3) \\
+ & & (a = (t_a,R_a),b = (t_b,R_b)) & \mapsto & b \ominus
+ a = (t_b - t_a, \log(R_a^{-1} R_b) \\ \end{array} \f]
- SE3Representation:
\f[ \begin{array}{ccccc}
\ominus & : & SE(3)^2 & \to & \mathfrak{se}(3) \\
@@ -60,104 +60,109 @@ enum Representation_t {
*/
template <Representation_t representation = R3xSO3Representation>
-class SOT_CORE_DLLAPI FeaturePose
- : public FeatureAbstract
-{
+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; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- public:
+public:
/*! \name Input signals
@{
*/
/// Input pose of <em>Joint A</em> wrt to world frame.
- dg::SignalPtr< MatrixHomogeneous, int > oMja;
+ dg::SignalPtr<MatrixHomogeneous, int> oMja;
/// Input pose of <em>Frame A</em> wrt to <em>Joint A</em>.
- dg::SignalPtr< MatrixHomogeneous, int > jaMfa;
+ dg::SignalPtr<MatrixHomogeneous, int> jaMfa;
/// Input pose of <em>Joint B</em> wrt to world frame.
- dg::SignalPtr< MatrixHomogeneous, int > oMjb;
+ dg::SignalPtr<MatrixHomogeneous, int> oMjb;
/// Input pose of <em>Frame B</em> wrt to <em>Joint B</em>.
- dg::SignalPtr< MatrixHomogeneous, int > jbMfb;
+ dg::SignalPtr<MatrixHomogeneous, int> jbMfb;
/// Jacobian of the input <em>Joint A</em>, expressed in <em>Joint A</em>
- dg::SignalPtr< Matrix,int > jaJja;
+ dg::SignalPtr<Matrix, int> jaJja;
/// Jacobian of the input <em>Joint B</em>, expressed in <em>Joint B</em>
- dg::SignalPtr< Matrix,int > jbJjb;
+ dg::SignalPtr<Matrix, int> jbJjb;
/// The desired pose of <em>Frame B</em> wrt to <em>Frame A</em>.
- dg::SignalPtr< MatrixHomogeneous, int > faMfbDes;
- /// The desired velocity of <em>Frame B</em> wrt to <em>Frame A</em>. The value is expressed in <em>Frame A</em>.
- dg::SignalPtr< Vector, int > faNufafbDes;
+ dg::SignalPtr<MatrixHomogeneous, int> faMfbDes;
+ /// The desired velocity of <em>Frame B</em> wrt to <em>Frame A</em>. The
+ /// value is expressed in <em>Frame A</em>.
+ dg::SignalPtr<Vector, int> faNufafbDes;
/*! @} */
/*! \name Output signals
@{
*/
/// Pose of <em>Frame B</em> wrt to <em>Frame A</em>.
- SignalTimeDependent< MatrixHomogeneous, int > faMfb;
+ SignalTimeDependent<MatrixHomogeneous, int> faMfb;
/// Pose of <em>Frame B</em> wrt to <em>Frame A</em>.
/// It is expressed as a translation followed by a quaternion.
- SignalTimeDependent< Vector7, int > q_faMfb;
+ SignalTimeDependent<Vector7, int> q_faMfb;
/// Desired pose of <em>Frame B</em> wrt to <em>Frame A</em>.
/// It is expressed as a translation followed by a quaternion.
- SignalTimeDependent< Vector7, int > q_faMfbDes;
+ SignalTimeDependent<Vector7, int> q_faMfbDes;
/*! @} */
- using FeatureAbstract::selectionSIN;
- using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::errorSOUT;
+ using FeatureAbstract::jacobianSOUT;
+ using FeatureAbstract::selectionSIN;
/*! \name Dealing with the reference value to be reach with this feature.
@{ */
DECLARE_NO_REFERENCE();
/*! @} */
- public:
- FeaturePose( const std::string& name );
- virtual ~FeaturePose( void ) {}
+public:
+ FeaturePose(const std::string &name);
+ virtual ~FeaturePose(void) {}
- virtual unsigned int& getDimension( unsigned int & dim, int time );
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
/// Computes \f$ {}^oM^{-1}_{fa} {}^oM_{fb} \ominus {}^{fa}M^*_{fb} \f$
- virtual dg::Vector& computeError( dg::Vector& res,int time );
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
/// Computes \f$ \frac{\partial\ominus}{\partial b} X {}^{fa}\nu^*_{fafb} \f$.
/// There are two different cases, depending on the representation:
- /// - R3xSO3Representation: \f$ X = \left( \begin{array}{cc} I_3 & [ {}^{fa}t_{fb} ] \\ 0_3 & {{}^{fa}R^*_{fb}}^T \end{array} \right) \f$
- /// - SE3Representation: \f$ X = {{}^{fa}X^*_{fb}}^{-1} \f$ (see pinocchio::SE3Base<Scalar,Options>::toActionMatrix)
- virtual dg::Vector& computeErrorDot( dg::Vector& res,int time );
- /// Computes \f$ \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}} {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \f$.
- /// There are two different cases, depending on the representation:
- /// - R3xSO3Representation: \f$ Y = \left( \begin{array}{cc} {{}^{fa}R_{fb}} & 0_3 \\ 0_3 & I_3 \end{array} \right) \f$
+ /// - R3xSO3Representation: \f$ X = \left( \begin{array}{cc} I_3 & [
+ /// {}^{fa}t_{fb} ] \\ 0_3 & {{}^{fa}R^*_{fb}}^T \end{array} \right) \f$
+ /// - SE3Representation: \f$ X = {{}^{fa}X^*_{fb}}^{-1} \f$ (see
+ /// pinocchio::SE3Base<Scalar,Options>::toActionMatrix)
+ virtual dg::Vector &computeErrorDot(dg::Vector &res, int time);
+ /// Computes \f$ \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}}
+ /// {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \f$. There are two
+ /// different cases, depending on the representation:
+ /// - R3xSO3Representation: \f$ Y = \left( \begin{array}{cc} {{}^{fa}R_{fb}} &
+ /// 0_3 \\ 0_3 & I_3 \end{array} \right) \f$
/// - SE3Representation: \f$ Y = I_6 \f$
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
/** Static Feature selection. */
- inline static Flags selectX( void ) { return FLAG_LINE_1; }
- inline static Flags selectY( void ) { return FLAG_LINE_2; }
- inline static Flags selectZ( void ) { return FLAG_LINE_3; }
- inline static Flags selectRX( void ) { return FLAG_LINE_4; }
- inline static Flags selectRY( void ) { return FLAG_LINE_5; }
- inline static Flags selectRZ( void ) { return FLAG_LINE_6; }
+ inline static Flags selectX(void) { return FLAG_LINE_1; }
+ inline static Flags selectY(void) { return FLAG_LINE_2; }
+ inline static Flags selectZ(void) { return FLAG_LINE_3; }
+ inline static Flags selectRX(void) { return FLAG_LINE_4; }
+ inline static Flags selectRY(void) { return FLAG_LINE_5; }
+ inline static Flags selectRZ(void) { return FLAG_LINE_6; }
- inline static Flags selectTranslation( void ) { return Flags(7); }
- inline static Flags selectRotation( void ) { return Flags(56); }
+ inline static Flags selectTranslation(void) { return Flags(7); }
+ inline static Flags selectRotation(void) { return Flags(56); }
- virtual void display( std::ostream& os ) const;
+ virtual void display(std::ostream &os) const;
- public:
- void servoCurrentPosition( void );
- private:
- MatrixHomogeneous& computefaMfb (MatrixHomogeneous& res, int time);
- Vector7& computeQfaMfb (Vector7& res, int time);
- Vector7& computeQfaMfbDes (Vector7& res, int time);
+public:
+ void servoCurrentPosition(void);
+
+private:
+ MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
+ Vector7 &computeQfaMfb(Vector7 &res, int time);
+ Vector7 &computeQfaMfbDes(Vector7 &res, int time);
/// \todo Intermediate variables for internal computations
-} ;
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__
diff --git a/include/sot/core/feature-posture.hh b/include/sot/core/feature-posture.hh
index 0e782e38..4d7254e3 100644
--- a/include/sot/core/feature-posture.hh
+++ b/include/sot/core/feature-posture.hh
@@ -18,68 +18,66 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_posture_EXPORTS)
-# define SOTFEATUREPOSTURE_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATUREPOSTURE_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_posture_EXPORTS)
+#define SOTFEATUREPOSTURE_EXPORT __declspec(dllexport)
#else
-# define SOTFEATUREPOSTURE_EXPORT
+#define SOTFEATUREPOSTURE_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTFEATUREPOSTURE_EXPORT
#endif
namespace dynamicgraph {
- namespace sot {
- using command::Command;
- using command::Value;
+namespace sot {
+using command::Command;
+using command::Value;
- /*! @class dynamicgraph::sot::FeaturePosture feature-posture.hh
- * Feature that observes the posture of the robot, ie whose Jacobian is the
- * identity, or slices of the identity. This feature can be exactly
- * obtained with a generic posture, given the identity matrix as the input
- * Jacobian, the identity matrix. It is even prefereable, as the reference
- * value is then given by a signal, which can be reevalutated at each
- * iteration, for example to track a reference trajectory in the
- * configuration space. See for example the toFlag python function in the
- * sot-dyninv module to nicely selec the posture DOF.
- */
+/*! @class dynamicgraph::sot::FeaturePosture feature-posture.hh
+ * Feature that observes the posture of the robot, ie whose Jacobian is the
+ * identity, or slices of the identity. This feature can be exactly
+ * obtained with a generic posture, given the identity matrix as the input
+ * Jacobian, the identity matrix. It is even prefereable, as the reference
+ * value is then given by a signal, which can be reevalutated at each
+ * iteration, for example to track a reference trajectory in the
+ * configuration space. See for example the toFlag python function in the
+ * sot-dyninv module to nicely selec the posture DOF.
+ */
- class SOTFEATUREPOSTURE_EXPORT FeaturePosture
- : public FeatureAbstract
- {
- class SelectDof;
- friend class SelectDof;
+class SOTFEATUREPOSTURE_EXPORT FeaturePosture : public FeatureAbstract {
+ class SelectDof;
+ friend class SelectDof;
- DYNAMIC_GRAPH_ENTITY_DECL();
+ DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- typedef dynamicgraph::SignalPtr<dg::Vector, int> signalIn_t;
- typedef dynamicgraph::SignalTimeDependent<dg::Vector, int> signalOut_t;
+public:
+ typedef dynamicgraph::SignalPtr<dg::Vector, int> signalIn_t;
+ typedef dynamicgraph::SignalTimeDependent<dg::Vector, int> signalOut_t;
- DECLARE_NO_REFERENCE;
+ DECLARE_NO_REFERENCE;
- explicit FeaturePosture (const std::string& name);
- virtual ~FeaturePosture ();
- virtual unsigned int& getDimension( unsigned int& res,int );
- void selectDof (unsigned dofId, bool control);
+ explicit FeaturePosture(const std::string &name);
+ virtual ~FeaturePosture();
+ virtual unsigned int &getDimension(unsigned int &res, int);
+ void selectDof(unsigned dofId, bool control);
- protected:
+protected:
+ virtual dg::Vector &computeError(dg::Vector &res, int);
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int);
+ virtual dg::Vector &computeActivation(dg::Vector &res, int);
+ virtual dg::Vector &computeErrorDot(dg::Vector &res, int time);
- virtual dg::Vector& computeError( dg::Vector& res, int );
- virtual dg::Matrix& computeJacobian( dg::Matrix& res, int );
- virtual dg::Vector& computeActivation( dg::Vector& res, int );
- virtual dg::Vector& computeErrorDot (dg::Vector& res,int time);
+ signalIn_t state_;
+ signalIn_t posture_;
+ signalIn_t postureDot_;
+ signalOut_t error_;
+ dg::Matrix jacobian_;
- signalIn_t state_;
- signalIn_t posture_;
- signalIn_t postureDot_;
- signalOut_t error_;
- dg::Matrix jacobian_;
- private:
- std::vector <bool> activeDofs_;
- std::size_t nbActiveDofs_;
- }; // class FeaturePosture
- } // namespace sot
+private:
+ std::vector<bool> activeDofs_;
+ std::size_t nbActiveDofs_;
+}; // 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 f329dbeb..66df85ac 100644
--- a/include/sot/core/feature-task.hh
+++ b/include/sot/core/feature-task.hh
@@ -15,61 +15,58 @@
/* --------------------------------------------------------------------- */
/* SOT */
+#include <sot/core/exception-task.hh>
#include <sot/core/feature-generic.hh>
#include <sot/core/task-abstract.hh>
-#include <sot/core/exception-task.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_task_EXPORTS)
-# define SOTFEATURETASK_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATURETASK_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_task_EXPORTS)
+#define SOTFEATURETASK_EXPORT __declspec(dllexport)
+#else
+#define SOTFEATURETASK_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTFEATURETASK_EXPORT
+#define SOTFEATURETASK_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTFEATURETASK_EXPORT FeatureTask
- : public FeatureGeneric
-{
+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; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- protected:
- TaskAbstract * taskPtr;
+protected:
+ TaskAbstract *taskPtr;
/* --- SIGNALS ------------------------------------------------------------ */
- public:
-
- public:
-
+public:
+public:
/*! \brief Default constructor */
- FeatureTask( const std::string& name );
+ FeatureTask(const std::string &name);
/*! \brief Default destructor */
- virtual ~FeatureTask( void ) {}
+ virtual ~FeatureTask(void) {}
/*! \brief Display the information related to this task implementation. */
- virtual void display( std::ostream& os ) const;
-
-} ;
+ virtual void display(std::ostream &os) const;
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_TASK_HH__
diff --git a/include/sot/core/feature-vector3.hh b/include/sot/core/feature-vector3.hh
index 6313dbd0..d5d42eba 100644
--- a/include/sot/core/feature-vector3.hh
+++ b/include/sot/core/feature-vector3.hh
@@ -15,73 +15,70 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/feature-abstract.hh>
#include <sot/core/exception-task.hh>
+#include <sot/core/feature-abstract.hh>
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_vector3_EXPORTS)
-# define SOTFEATUREVECTOR3_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATUREVECTOR3_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_vector3_EXPORTS)
+#define SOTFEATUREVECTOR3_EXPORT __declspec(dllexport)
+#else
+#define SOTFEATUREVECTOR3_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTFEATUREVECTOR3_EXPORT
+#define SOTFEATUREVECTOR3_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
\class FeatureVector3
\brief Class that defines point-3d control feature
*/
-class SOTFEATUREVECTOR3_EXPORT FeatureVector3
-: public FeatureAbstract
-{
+class SOTFEATUREVECTOR3_EXPORT FeatureVector3 : public FeatureAbstract {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
DECLARE_NO_REFERENCE;
- protected:
-
+protected:
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalPtr< dg::Vector,int > vectorSIN;
- dg::SignalPtr< MatrixHomogeneous,int > positionSIN;
- dg::SignalPtr< dg::Matrix,int > articularJacobianSIN;
- dg::SignalPtr< dg::Vector,int > positionRefSIN;
+public:
+ dg::SignalPtr<dg::Vector, int> vectorSIN;
+ dg::SignalPtr<MatrixHomogeneous, int> positionSIN;
+ dg::SignalPtr<dg::Matrix, int> articularJacobianSIN;
+ dg::SignalPtr<dg::Vector, int> positionRefSIN;
- using FeatureAbstract::selectionSIN;
- using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::errorSOUT;
+ using FeatureAbstract::jacobianSOUT;
+ using FeatureAbstract::selectionSIN;
- public:
- FeatureVector3( const std::string& name );
- virtual ~FeatureVector3( void ) {}
-
- virtual unsigned int& getDimension( unsigned int & dim, int time );
-
- virtual dg::Vector& computeError( dg::Vector& res,int time );
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+public:
+ FeatureVector3(const std::string &name);
+ virtual ~FeatureVector3(void) {}
- virtual void display( std::ostream& os ) const;
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
-} ;
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
-} /* namespace sot */} /* namespace dynamicgraph */
+ virtual void display(std::ostream &os) const;
+};
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_VECTOR3_HH__
diff --git a/include/sot/core/feature-visual-point.hh b/include/sot/core/feature-visual-point.hh
index 467139f3..7bebd3d2 100644
--- a/include/sot/core/feature-visual-point.hh
+++ b/include/sot/core/feature-visual-point.hh
@@ -15,28 +15,29 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/feature-abstract.hh>
#include <sot/core/exception-task.hh>
+#include <sot/core/feature-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (feature_visual_point_EXPORTS)
-# define SOTFEATUREVISUALPOINT_EXPORT __declspec(dllexport)
-# else
-# define SOTFEATUREVISUALPOINT_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(feature_visual_point_EXPORTS)
+#define SOTFEATUREVISUALPOINT_EXPORT __declspec(dllexport)
+#else
+#define SOTFEATUREVISUALPOINT_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTFEATUREVISUALPOINT_EXPORT
+#define SOTFEATUREVISUALPOINT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
@@ -44,51 +45,48 @@ namespace dg = dynamicgraph;
\brief Class that defines 2D visualPoint visual feature
*/
class SOTFEATUREVISUALPOINT_EXPORT FeatureVisualPoint
- : public FeatureAbstract, public FeatureReferenceHelper<FeatureVisualPoint>
-{
+ : public FeatureAbstract,
+ public FeatureReferenceHelper<FeatureVisualPoint> {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- protected:
+protected:
dg::Matrix L;
-
-
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalPtr< dg::Vector,int > xySIN;
+public:
+ dg::SignalPtr<dg::Vector, int> xySIN;
/** FeatureVisualPoint depth (required to compute the interaction matrix)
* default Z = 1m. */
- dg::SignalPtr< double,int > ZSIN;
- dg::SignalPtr< dg::Matrix,int > articularJacobianSIN;
+ dg::SignalPtr<double, int> ZSIN;
+ dg::SignalPtr<dg::Matrix, int> articularJacobianSIN;
- using FeatureAbstract::selectionSIN;
- using FeatureAbstract::jacobianSOUT;
using FeatureAbstract::errorSOUT;
+ using FeatureAbstract::jacobianSOUT;
+ using FeatureAbstract::selectionSIN;
DECLARE_REFERENCE_FUNCTIONS(FeatureVisualPoint);
- public:
- FeatureVisualPoint( const std::string& name );
- virtual ~FeatureVisualPoint( void ) {}
+public:
+ FeatureVisualPoint(const std::string &name);
+ virtual ~FeatureVisualPoint(void) {}
- virtual unsigned int& getDimension( unsigned int & dim, int time );
+ virtual unsigned int &getDimension(unsigned int &dim, int time);
- virtual dg::Vector& computeError( dg::Vector& res,int time );
- virtual dg::Matrix& computeJacobian( dg::Matrix& res,int time );
+ virtual dg::Vector &computeError(dg::Vector &res, int time);
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time);
/** Static Feature selection. */
- inline static Flags selectX( void ) { return FLAG_LINE_1; }
- inline static Flags selectY( void ) { return FLAG_LINE_2; }
-
- virtual void display( std::ostream& os ) const;
-
+ inline static Flags selectX(void) { return FLAG_LINE_1; }
+ inline static Flags selectY(void) { return FLAG_LINE_2; }
-} ;
+ virtual void display(std::ostream &os) const;
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_FEATURE_VISUALPOINT_HH__
diff --git a/include/sot/core/filter-differentiator.hh b/include/sot/core/filter-differentiator.hh
index 81dc486b..634d20fe 100644
--- a/include/sot/core/filter-differentiator.hh
+++ b/include/sot/core/filter-differentiator.hh
@@ -20,14 +20,14 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (low_pass_filter_EXPORTS)
-# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllexport)
-# else
-# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(low_pass_filter_EXPORTS)
+#define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllexport)
#else
-# define SOTFILTERDIFFERENTIATOR_EXPORT
+#define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTFILTERDIFFERENTIATOR_EXPORT
#endif
//#define VP_DEBUG 1 /// enable debug output
@@ -39,93 +39,86 @@
/* HELPER */
#include <dynamic-graph/signal-helper.h>
-#include <sot/core/stop-watch.hh>
#include <sot/core/causal-filter.hh>
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph {
- namespace sot {
- /** \addtogroup Filters
- \section subsec_filterdiff FilterDifferentiator
- This Entity takes as inputs a signal and applies a low pass filter (implemented through CasualFilter)
- and computes finite difference derivative.
-
- The input signal is provided through m_xSIN (an entity signal).
- The filtered signal is given through m_x_filteredSOUT.
- The first derivative of the filtered signal is provided with m_dxSOUT.
- The second derivative of the filtered signal is provided with m_ddxSOUT.
- */
- class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
- :public ::dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public: /* --- SIGNALS --- */
- /// Input signals
- DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
- /// Output signal x_filtered
- DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
- DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
- DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
-
- /// The following inner signals are used because this entity has
- /// some output signals
- /// whose related quantities are computed at the same time by the
- /// same algorithm
- /// To avoid the risk of recomputing the same things twice,
- /// we create an inner signal that groups together
- /// all the quantities that are computed together.
- /// Then the single output signals will depend
- /// on this inner signal, which is the one triggering the computations.
- /// Inner signals are not exposed, so that nobody can access them.
-
- /// This signal contains the estimated positions, velocities and
- /// accelerations.
- DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
-
- protected:
-
- double m_dt; /// sampling timestep of the input signal
- int m_x_size;
-
- /// polynomial-fitting filters
- CausalFilter* m_filter;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /** --- CONSTRUCTOR ---- */
- FilterDifferentiator( const std::string & name );
-
- /** Initialize the FilterDifferentiator.
- * @param timestep Period (in seconds) after which
- * the sensors' data are updated.
- * @param sigSize Size of the input signal.
- * @param delay Delay (in seconds) introduced by the estimation.
- * This should be a multiple of timestep.
- * @note The estimationDelay is half of the length of the
- * window used for the
- * polynomial fitting. The larger the delay,
- * the smoother the estimations.
- */
- void init(const double ×tep,
- const int& xSize,
- const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator);
-
- void switch_filter(const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator);
-
-
- protected:
-
- public: /* --- ENTITY INHERITANCE --- */
- virtual void display( std::ostream& os ) const;
-
- }; // class FilterDifferentiator
-
- } // namespace sot
+namespace sot {
+/** \addtogroup Filters
+ \section subsec_filterdiff FilterDifferentiator
+ This Entity takes as inputs a signal and applies a low pass filter
+ (implemented through CasualFilter) and computes finite difference derivative.
+
+ The input signal is provided through m_xSIN (an entity signal).
+ The filtered signal is given through m_x_filteredSOUT.
+ The first derivative of the filtered signal is provided with m_dxSOUT.
+ The second derivative of the filtered signal is provided with m_ddxSOUT.
+ */
+class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
+ : public ::dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+public: /* --- SIGNALS --- */
+ /// Input signals
+ DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
+ /// Output signal x_filtered
+ DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
+ DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
+
+ /// The following inner signals are used because this entity has
+ /// some output signals
+ /// whose related quantities are computed at the same time by the
+ /// same algorithm
+ /// To avoid the risk of recomputing the same things twice,
+ /// we create an inner signal that groups together
+ /// all the quantities that are computed together.
+ /// Then the single output signals will depend
+ /// on this inner signal, which is the one triggering the computations.
+ /// Inner signals are not exposed, so that nobody can access them.
+
+ /// This signal contains the estimated positions, velocities and
+ /// accelerations.
+ DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
+
+protected:
+ double m_dt; /// sampling timestep of the input signal
+ int m_x_size;
+
+ /// polynomial-fitting filters
+ CausalFilter *m_filter;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /** --- CONSTRUCTOR ---- */
+ FilterDifferentiator(const std::string &name);
+
+ /** Initialize the FilterDifferentiator.
+ * @param timestep Period (in seconds) after which
+ * the sensors' data are updated.
+ * @param sigSize Size of the input signal.
+ * @param delay Delay (in seconds) introduced by the estimation.
+ * This should be a multiple of timestep.
+ * @note The estimationDelay is half of the length of the
+ * window used for the
+ * polynomial fitting. The larger the delay,
+ * the smoother the estimations.
+ */
+ void init(const double ×tep, const int &xSize,
+ const Eigen::VectorXd &filter_numerator,
+ const Eigen::VectorXd &filter_denominator);
+
+ void switch_filter(const Eigen::VectorXd &filter_numerator,
+ const Eigen::VectorXd &filter_denominator);
+
+protected:
+public: /* --- ENTITY INHERITANCE --- */
+ virtual void display(std::ostream &os) const;
+
+}; // class FilterDifferentiator
+
+} // namespace sot
} // namespace dynamicgraph
-
-
#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 b9bb2d47..db2b84bc 100644
--- a/include/sot/core/fir-filter-impl.hh
+++ b/include/sot/core/fir-filter-impl.hh
@@ -16,35 +16,35 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (fir_filter_EXPORTS)
-# define FIL_FILTER_EXPORT __declspec(dllexport)
-# else
-# define FIL_FILTER_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(fir_filter_EXPORTS)
+#define FIL_FILTER_EXPORT __declspec(dllexport)
#else
-# define FIL_FILTER_EXPORT
+#define FIL_FILTER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define FIL_FILTER_EXPORT
#endif
-# ifdef WIN32
-# 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 ) \
- typedef FIRFilter<sotSigType,sotCoefType> className;
-# endif // WIN32
+#ifdef WIN32
+#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) \
+ typedef FIRFilter<sotSigType, sotCoefType> className;
+#endif // WIN32
namespace dynamicgraph {
- namespace sot {
+namespace sot {
-DECLARE_SPECIFICATION(FIRFilterDoubleDouble,double,double)
-DECLARE_SPECIFICATION(FIRFilterVectorDouble,Vector,double)
-DECLARE_SPECIFICATION(FIRFilterVectorMatrix,Vector,Matrix)
+DECLARE_SPECIFICATION(FIRFilterDoubleDouble, double, double)
+DECLARE_SPECIFICATION(FIRFilterVectorDouble, Vector, double)
+DECLARE_SPECIFICATION(FIRFilterVectorMatrix, Vector, Matrix)
- } // namespace sot
+} // namespace sot
} // namespace dynamicgraph
#endif
diff --git a/include/sot/core/fir-filter.hh b/include/sot/core/fir-filter.hh
index 6cef80be..b543dbdd 100644
--- a/include/sot/core/fir-filter.hh
+++ b/include/sot/core/fir-filter.hh
@@ -12,264 +12,226 @@
#include <cassert>
-#include <vector>
#include <algorithm>
#include <iterator>
+#include <vector>
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
-#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/entity.h>
namespace dg = dynamicgraph;
namespace dynamicgraph {
- namespace sot {
-
- namespace detail
- {
- // GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
- // 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:
- circular_buffer(): buf(1), start(0), numel(0) {}
- void push_front(const T& data) {
- if(start){ --start; } else { start = buf.size()-1; }
- buf[start] = data;
- if(numel < buf.size()){ ++numel; }
- }
- void reset_capacity(size_t n) {
- buf.resize(n);
- start = 0;
- numel = 0;
- }
- void reset_capacity(size_t n, const T& el) {
- buf.clear();
- buf.resize(n, el);
- start = 0;
- numel = 0;
- }
- T& operator[](size_t i) {
- assert((i<numel) && "Youre accessing an empty buffer");
- size_t index = (start+i) % buf.size();
- return buf[index];
- }
- size_t size() const { return numel; }
- private:
- std::vector<T> buf;
- size_t start;
- size_t numel;
- }; // class circular_buffer
- } // namespace detail
-
- 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:
- SetElement (FIRFilter < sigT, coefT > & entity,
- const std::string& docstring);
- Value doExecute ();
- }; // class SetElement
-
- 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
-
- using ::dynamicgraph::command::Setter;
- using ::dynamicgraph::command::Getter;
-
- template < class sigT, class coefT >
- class FIRFilter
- : public Entity
- {
- public:
- virtual const std::string& getClassName() const
- {
- return Entity::getClassName();
- }
- static std::string getTypeName( void )
- {
- return "Unknown";
- }
- static const std::string CLASS_NAME;
-
- std::string getDocString () const
- {
- return
- "Finite impulse response filter\n"
- "\n"
- " Provide the following sum in output signal:\n"
- " N \n"
- " __ \n"
- " y (n) = \\ c s (n-i) \n"
- " /_ i \n"
- " i=0 \n"
- " \n"
- " where\n"
- " - c_i are coefficients stored in an array\n"
- " - N is the size of the array\n"
- " - s is the input signal.\n";
- }
- public:
- FIRFilter( const std::string& name )
- : 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";
- 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";
- addCommand ("getElement",
- new command::GetElement <sigT, coefT>
- (*this, docstring));
- 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";
- addCommand ("getSize",
- new Getter < FIRFilter, unsigned >
- (*this, &FIRFilter::getBufferSize, docstring));
-
- }
-
- virtual ~FIRFilter() {}
-
- virtual sigT& compute( sigT& res,int time )
- {
- const sigT& in = SIN.access( time );
- reset_signal( res, in );
- data.push_front( in );
-
- size_t SIZE = std::min(data.size(), coefs.size());
- for(size_t i = 0; i < SIZE; ++i) {
- res += coefs[i] * data[i];
- }
-
- return res;
- }
-
- void resizeBuffer (const unsigned int& size)
- {
- size_t s = static_cast <size_t> (size);
- data.reset_capacity(s);
- coefs.resize (s);
- }
-
- unsigned int getBufferSize () const
- {
- return static_cast <unsigned int> (coefs.size ());
-
- }
-
- void setElement (const unsigned int& rank, const coefT& coef)
- {
- coefs [rank] = coef;
- }
-
- coefT getElement (const unsigned int& rank) const
- {
- return coefs [rank];
- }
-
- static void reset_signal(sigT& /*res*/, const sigT& /*sample*/ ) { }
-
- public:
- SignalPtr<sigT, int> SIN;
- SignalTimeDependent<sigT, int> SOUT;
-
- private:
- std::vector<coefT> coefs;
- detail::circular_buffer<sigT> data;
- }; // class FIRFilter
-
- namespace command {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
- using ::dynamicgraph::command::ValueHelper;
-
- template <class sigT, class coefT>
- SetElement <sigT, coefT>::SetElement
- (FIRFilter <sigT, coefT>& entity,
- const std::string& docstring) :
- Command (entity, boost::assign::list_of (Value::UNSIGNED)
- (ValueHelper <coefT>::TypeID), docstring)
- {
- }
-
- 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();
- unsigned int rank = values [0].value ();
- coefT coef = values [1].value ();
- entity.setElement (rank, coef);
- return Value ();
- }
-
- template <class sigT, class coefT>
- 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 ()
- {
- 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 sot
+namespace sot {
+
+namespace detail {
+// GRMBL boost-sandox::circular_buffer smells... Why keep using 1.33?!
+// 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:
+ circular_buffer() : buf(1), start(0), numel(0) {}
+ void push_front(const T &data) {
+ if (start) {
+ --start;
+ } else {
+ start = buf.size() - 1;
+ }
+ buf[start] = data;
+ if (numel < buf.size()) {
+ ++numel;
+ }
+ }
+ void reset_capacity(size_t n) {
+ buf.resize(n);
+ start = 0;
+ numel = 0;
+ }
+ void reset_capacity(size_t n, const T &el) {
+ buf.clear();
+ buf.resize(n, el);
+ start = 0;
+ numel = 0;
+ }
+ T &operator[](size_t i) {
+ assert((i < numel) && "Youre accessing an empty buffer");
+ size_t index = (start + i) % buf.size();
+ return buf[index];
+ }
+ size_t size() const { return numel; }
+
+private:
+ std::vector<T> buf;
+ size_t start;
+ size_t numel;
+}; // class circular_buffer
+} // namespace detail
+
+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:
+ SetElement(FIRFilter<sigT, coefT> &entity, const std::string &docstring);
+ Value doExecute();
+}; // class SetElement
+
+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
+
+using ::dynamicgraph::command::Getter;
+using ::dynamicgraph::command::Setter;
+
+template <class sigT, class coefT> class FIRFilter : public Entity {
+public:
+ virtual const std::string &getClassName() const {
+ return Entity::getClassName();
+ }
+ static std::string getTypeName(void) { return "Unknown"; }
+ static const std::string CLASS_NAME;
+
+ std::string getDocString() const {
+ return "Finite impulse response filter\n"
+ "\n"
+ " Provide the following sum in output signal:\n"
+ " N \n"
+ " __ \n"
+ " y (n) = \\ c s (n-i) \n"
+ " /_ i \n"
+ " i=0 \n"
+ " \n"
+ " where\n"
+ " - c_i are coefficients stored in an array\n"
+ " - N is the size of the array\n"
+ " - s is the input signal.\n";
+ }
+
+public:
+ FIRFilter(const std::string &name)
+ : 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";
+ 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";
+ addCommand("getElement",
+ new command::GetElement<sigT, coefT>(*this, docstring));
+ 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";
+ addCommand("getSize", new Getter<FIRFilter, unsigned>(
+ *this, &FIRFilter::getBufferSize, docstring));
+ }
+
+ virtual ~FIRFilter() {}
+
+ virtual sigT &compute(sigT &res, int time) {
+ const sigT &in = SIN.access(time);
+ reset_signal(res, in);
+ data.push_front(in);
+
+ size_t SIZE = std::min(data.size(), coefs.size());
+ for (size_t i = 0; i < SIZE; ++i) {
+ res += coefs[i] * data[i];
+ }
+
+ return res;
+ }
+
+ void resizeBuffer(const unsigned int &size) {
+ size_t s = static_cast<size_t>(size);
+ data.reset_capacity(s);
+ coefs.resize(s);
+ }
+
+ unsigned int getBufferSize() const {
+ return static_cast<unsigned int>(coefs.size());
+ }
+
+ void setElement(const unsigned int &rank, const coefT &coef) {
+ coefs[rank] = coef;
+ }
+
+ coefT getElement(const unsigned int &rank) const { return coefs[rank]; }
+
+ static void reset_signal(sigT & /*res*/, const sigT & /*sample*/) {}
+
+public:
+ SignalPtr<sigT, int> SIN;
+ SignalTimeDependent<sigT, int> SOUT;
+
+private:
+ std::vector<coefT> coefs;
+ detail::circular_buffer<sigT> data;
+}; // class FIRFilter
+
+namespace command {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+using ::dynamicgraph::command::ValueHelper;
+
+template <class sigT, class coefT>
+SetElement<sigT, coefT>::SetElement(FIRFilter<sigT, coefT> &entity,
+ const std::string &docstring)
+ : Command(
+ entity,
+ boost::assign::list_of(Value::UNSIGNED)(ValueHelper<coefT>::TypeID),
+ docstring) {}
+
+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();
+ unsigned int rank = values[0].value();
+ coefT coef = values[1].value();
+ entity.setElement(rank, coef);
+ return Value();
+}
+
+template <class sigT, class coefT>
+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() {
+ 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 sot
} // namespace dynamicgraph
#endif
diff --git a/include/sot/core/flags.hh b/include/sot/core/flags.hh
index ac2c7236..75c09d98 100644
--- a/include/sot/core/flags.hh
+++ b/include/sot/core/flags.hh
@@ -15,8 +15,8 @@
/* --------------------------------------------------------------------- */
/* STD */
-#include <vector>
#include <ostream>
+#include <vector>
/* SOT */
#include "sot/core/api.hh"
@@ -26,70 +26,61 @@
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
-
- class SOT_CORE_EXPORT Flags
- {
- protected:
- std::vector<char> flags;
- bool reverse;
-
- char operator[](const unsigned int& i) const;
-
-
- public:
-
- Flags(const bool& b=false);
- Flags(const char& c);
- Flags(const int& c4);
-
- void add(const char& c);
- void add(const int& c4);
-
- Flags operator!(void) const;
- SOT_CORE_EXPORT friend Flags operator&(const Flags& f1,const Flags& f2);
- SOT_CORE_EXPORT friend Flags operator|(const Flags& f1,const Flags& f2);
- Flags& operator&=(const Flags& f2);
- Flags& operator|=(const Flags& f2);
-
- SOT_CORE_EXPORT friend Flags operator&(const Flags& f1,const bool& b);
- SOT_CORE_EXPORT friend Flags operator|(const Flags& f1,const bool& b);
- Flags& operator&=(const bool& b);
- Flags& operator|=(const bool& b);
-
- SOT_CORE_EXPORT friend std::ostream& operator<<(std::ostream& os, const Flags& fl);
- SOT_CORE_EXPORT friend char operator>>(const Flags& flags, const int& i);
- SOT_CORE_EXPORT friend std::istream& operator>>(std::istream& is, Flags& fl);
- bool operator()(const int& i) const;
-
- operator bool(void) const;
-
- void unset(const unsigned int & i);
- void set(const unsigned int & i);
-
- public: /* Selec "matlab-style" : 1:15, 1:, :45 ... */
-
- static void readIndexMatlab(std::istream & iss,
- unsigned int & indexStart,
- unsigned int & indexEnd,
- bool& unspecifiedEnd);
- static Flags readIndexMatlab(std::istream & iss);
-
-
-
-
- };
-
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_1;
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_2;
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_3;
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_4;
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_5;
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_6;
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_7;
- SOT_CORE_EXPORT extern const Flags FLAG_LINE_8;
-
- } // namespace sot
+namespace sot {
+
+class SOT_CORE_EXPORT Flags {
+protected:
+ std::vector<char> flags;
+ bool reverse;
+
+ char operator[](const unsigned int &i) const;
+
+public:
+ Flags(const bool &b = false);
+ Flags(const char &c);
+ Flags(const int &c4);
+
+ void add(const char &c);
+ void add(const int &c4);
+
+ Flags operator!(void) const;
+ SOT_CORE_EXPORT friend Flags operator&(const Flags &f1, const Flags &f2);
+ SOT_CORE_EXPORT friend Flags operator|(const Flags &f1, const Flags &f2);
+ Flags &operator&=(const Flags &f2);
+ Flags &operator|=(const Flags &f2);
+
+ SOT_CORE_EXPORT friend Flags operator&(const Flags &f1, const bool &b);
+ SOT_CORE_EXPORT friend Flags operator|(const Flags &f1, const bool &b);
+ Flags &operator&=(const bool &b);
+ Flags &operator|=(const bool &b);
+
+ SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const Flags &fl);
+ SOT_CORE_EXPORT friend char operator>>(const Flags &flags, const int &i);
+ SOT_CORE_EXPORT friend std::istream &operator>>(std::istream &is, Flags &fl);
+ bool operator()(const int &i) const;
+
+ operator bool(void) const;
+
+ void unset(const unsigned int &i);
+ void set(const unsigned int &i);
+
+public: /* Selec "matlab-style" : 1:15, 1:, :45 ... */
+ static void readIndexMatlab(std::istream &iss, unsigned int &indexStart,
+ unsigned int &indexEnd, bool &unspecifiedEnd);
+ static Flags readIndexMatlab(std::istream &iss);
+};
+
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_1;
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_2;
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_3;
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_4;
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_5;
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_6;
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_7;
+SOT_CORE_EXPORT extern const Flags FLAG_LINE_8;
+
+} // namespace sot
} // namespace dynamicgraph
#endif /* #ifndef __SOT_FLAGS_H */
diff --git a/include/sot/core/gain-adaptive.hh b/include/sot/core/gain-adaptive.hh
index c608c53b..33f705be 100644
--- a/include/sot/core/gain-adaptive.hh
+++ b/include/sot/core/gain-adaptive.hh
@@ -26,82 +26,72 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (gain_adaptive_EXPORTS)
-# define SOTGAINADAPTATIVE_EXPORT __declspec(dllexport)
-# else
-# define SOTGAINADAPTATIVE_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(gain_adaptive_EXPORTS)
+#define SOTGAINADAPTATIVE_EXPORT __declspec(dllexport)
#else
-# define SOTGAINADAPTATIVE_EXPORT
+#define SOTGAINADAPTATIVE_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTGAINADAPTATIVE_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTGAINADAPTATIVE_EXPORT GainAdaptive
-: public dg::Entity
-{
-
- public: /* --- CONSTANTS --- */
+class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dg::Entity {
+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:
+ virtual void display(std::ostream &os) const;
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+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 ---- */
-
- 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 --- */
-
- 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,
- const double& tanAt0 );
- void initFromPassingPoint( const double& valueAt0,
- const double& valueAtInfty,
- const double& errorReference,
- const double& valueAtReference );
- void forceConstant( void );
-
- public: /* --- SIGNALS --- */
- dg::SignalPtr<dg::Vector,int> errorSIN;
- dg::SignalTimeDependent<double,int> gainSOUT;
- protected:
- double& computeGain( double& res,int t );
-
- private:
+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 --- */
+ 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,
+ const double &tanAt0);
+ void initFromPassingPoint(const double &valueAt0, const double &valueAtInfty,
+ const double &errorReference,
+ const double &valueAtReference);
+ void forceConstant(void);
+
+public: /* --- SIGNALS --- */
+ dg::SignalPtr<dg::Vector, int> errorSIN;
+ dg::SignalTimeDependent<double, int> gainSOUT;
+
+protected:
+ double &computeGain(double &res, int t);
+
+private:
void addCommands();
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_GAIN_ADAPTATIVE_HH__
diff --git a/include/sot/core/gain-hyperbolic.hh b/include/sot/core/gain-hyperbolic.hh
index 72a9e8b1..eea9a130 100644
--- a/include/sot/core/gain-hyperbolic.hh
+++ b/include/sot/core/gain-hyperbolic.hh
@@ -20,49 +20,44 @@ namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/all-signals.h>
-#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (gain_hyperbolic_EXPORTS)
-# define SOTGAINHYPERBOLIC_EXPORT __declspec(dllexport)
-# else
-# define SOTGAINHYPERBOLIC_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(gain_hyperbolic_EXPORTS)
+#define SOTGAINHYPERBOLIC_EXPORT __declspec(dllexport)
+#else
+#define SOTGAINHYPERBOLIC_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTGAINHYPERBOLIC_EXPORT
+#define SOTGAINHYPERBOLIC_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic
-: public dg::Entity
-{
-
- public: /* --- CONSTANTS --- */
+class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic : public dg::Entity {
+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:
+ virtual void display(std::ostream &os) const;
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+protected:
/* Parameters of the hyperbolic-gain function:
* lambda (x) = a * exp (-b*x) + c. */
double coeff_a;
@@ -70,35 +65,29 @@ class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic
double coeff_c;
double coeff_d;
- 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 --- */
-
- inline void init( void ) { init( ZERO_DEFAULT,INFTY_DEFAULT,TAN_DEFAULT,0 ); }
- inline void init( const double& lambda ) { init( lambda,lambda,1.,0); }
- void init( const double& valueAt0,
- const double& valueAtInfty,
- const double& tanAt0,
- const double& decal0 );
- void forceConstant( void );
-
- public: /* --- SIGNALS --- */
- dg::SignalPtr<dg::Vector,int> errorSIN;
- dg::SignalTimeDependent<double,int> gainSOUT;
- protected:
- double& computeGain( double& res,int t );
+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 --- */
+ inline void init(void) { init(ZERO_DEFAULT, INFTY_DEFAULT, TAN_DEFAULT, 0); }
+ inline void init(const double &lambda) { init(lambda, lambda, 1., 0); }
+ void init(const double &valueAt0, const double &valueAtInfty,
+ const double &tanAt0, const double &decal0);
+ void forceConstant(void);
+
+public: /* --- SIGNALS --- */
+ dg::SignalPtr<dg::Vector, int> errorSIN;
+ dg::SignalTimeDependent<double, int> gainSOUT;
+
+protected:
+ double &computeGain(double &res, int t);
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_GAIN_HYPERBOLIC_HH__
diff --git a/include/sot/core/gradient-ascent.hh b/include/sot/core/gradient-ascent.hh
index c41a536e..fba98c76 100644
--- a/include/sot/core/gradient-ascent.hh
+++ b/include/sot/core/gradient-ascent.hh
@@ -13,15 +13,15 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include <sot/core/config.hh>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
+#include <sot/core/config.hh>
namespace dg = ::dynamicgraph;
namespace dynamicgraph {
- namespace sot {
+namespace sot {
/* --------------------------------------------------------------------- */
/* --- TRACER ---------------------------------------------------------- */
@@ -31,33 +31,29 @@ using dynamicgraph::Entity;
using dynamicgraph::SignalPtr;
using dynamicgraph::SignalTimeDependent;
-class SOT_CORE_DLLAPI GradientAscent
-: public Entity
-{
+class SOT_CORE_DLLAPI GradientAscent : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- SignalPtr< dg::Vector,int > gradientSIN;
- SignalPtr< double,int > learningRateSIN;
- SignalTimeDependent<int,int> refresherSINTERN;
- SignalTimeDependent<dg::Vector,int> valueSOUT;
+public:
+ SignalPtr<dg::Vector, int> gradientSIN;
+ SignalPtr<double, int> learningRateSIN;
+ SignalTimeDependent<int, int> refresherSINTERN;
+ SignalTimeDependent<dg::Vector, int> valueSOUT;
- public:
- GradientAscent( const std::string& n );
- virtual ~GradientAscent( void );
+public:
+ GradientAscent(const std::string &n);
+ virtual ~GradientAscent(void);
- protected:
-
- dg::Vector& update(dg::Vector& res, const int& inTime);
+protected:
+ dg::Vector &update(dg::Vector &res, const int &inTime);
dg::Vector value;
double alpha;
bool init;
-
};
- } /* namespace sot */
+} /* namespace sot */
} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TRACER_H__ */
diff --git a/include/sot/core/gripper-control.hh b/include/sot/core/gripper-control.hh
index 35f7488c..36739641 100644
--- a/include/sot/core/gripper-control.hh
+++ b/include/sot/core/gripper-control.hh
@@ -19,9 +19,9 @@
namespace dg = dynamicgraph;
/* SOT */
+#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/flags.hh>
-#include <dynamic-graph/all-signals.h>
/* STD */
#include <string>
@@ -30,21 +30,22 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (gripper_control_EXPORTS)
-# define SOTGRIPPERCONTROL_EXPORT __declspec(dllexport)
-# else
-# define SOTGRIPPERCONTROL_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(gripper_control_EXPORTS)
+#define SOTGRIPPERCONTROL_EXPORT __declspec(dllexport)
+#else
+#define SOTGRIPPERCONTROL_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTGRIPPERCONTROL_EXPORT
+#define SOTGRIPPERCONTROL_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
@@ -54,91 +55,82 @@ namespace dg = dynamicgraph;
kept
*
*/
-class SOTGRIPPERCONTROL_EXPORT GripperControl
-{
- protected:
-
+class SOTGRIPPERCONTROL_EXPORT GripperControl {
+protected:
double offset;
static const double OFFSET_DEFAULT;
//! \brief The multiplication
dg::Vector factor;
- public:
- GripperControl( void );
+public:
+ GripperControl(void);
//! \brief Computes the
// if the torque limit is reached, the normalized position is reduced by
// (offset)
- void computeIncrement( const dg::Vector& torques,
- const dg::Vector& torqueLimits,
- const dg::Vector& currentNormVel );
+ void computeIncrement(const dg::Vector &torques,
+ const dg::Vector &torqueLimits,
+ const dg::Vector ¤tNormVel);
//! \brief
- dg::Vector& computeDesiredPosition( const dg::Vector& currentPos,
- const dg::Vector& desiredPos,
- const dg::Vector& torques,
- const dg::Vector& torqueLimits,
- dg::Vector& referencePos );
+ dg::Vector &computeDesiredPosition(const dg::Vector ¤tPos,
+ const dg::Vector &desiredPos,
+ const dg::Vector &torques,
+ const dg::Vector &torqueLimits,
+ dg::Vector &referencePos);
/*! \brief select only some of the values of the vector fullsize,
- * based on the Flags vector.
- */
+ * based on the Flags vector.
+ */
- static dg::Vector& selector( const dg::Vector& fullsize,
- const Flags& selec,
- dg::Vector& desPos );
+ static dg::Vector &selector(const dg::Vector &fullsize, const Flags &selec,
+ dg::Vector &desPos);
};
/* --------------------------------------------------------------------- */
/* --- PLUGIN ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin
-:public dg::Entity, public GripperControl
-{
+class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin : public dg::Entity,
+ public GripperControl {
DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- bool calibrationStarted;
-
- public: /* --- CONSTRUCTION --- */
+public:
+ bool calibrationStarted;
- GripperControlPlugin( const std::string& name );
- virtual ~GripperControlPlugin( void );
+public: /* --- CONSTRUCTION --- */
+ GripperControlPlugin(const std::string &name);
+ virtual ~GripperControlPlugin(void);
/* --- DOCUMENTATION --- */
- virtual std::string getDocString () const;
-
- public: /* --- SIGNAL --- */
+ virtual std::string getDocString() const;
+public: /* --- SIGNAL --- */
/* --- INPUTS --- */
- dg::SignalPtr<dg::Vector,int> positionSIN;
- dg::SignalPtr<dg::Vector,int> positionDesSIN;
- dg::SignalPtr<dg::Vector,int> torqueSIN;
- dg::SignalPtr<dg::Vector,int> torqueLimitSIN;
- dg::SignalPtr<Flags,int> selectionSIN;
+ dg::SignalPtr<dg::Vector, int> positionSIN;
+ dg::SignalPtr<dg::Vector, int> positionDesSIN;
+ dg::SignalPtr<dg::Vector, int> torqueSIN;
+ dg::SignalPtr<dg::Vector, int> torqueLimitSIN;
+ dg::SignalPtr<Flags, int> selectionSIN;
/* --- INTERMEDIARY --- */
- dg::SignalPtr<dg::Vector,int> positionFullSizeSIN;
- dg::SignalTimeDependent<dg::Vector,int> positionReduceSOUT;
- dg::SignalPtr<dg::Vector,int> torqueFullSizeSIN;
- dg::SignalTimeDependent<dg::Vector,int> torqueReduceSOUT;
- dg::SignalPtr<dg::Vector,int> torqueLimitFullSizeSIN;
- dg::SignalTimeDependent<dg::Vector,int> torqueLimitReduceSOUT;
+ dg::SignalPtr<dg::Vector, int> positionFullSizeSIN;
+ dg::SignalTimeDependent<dg::Vector, int> positionReduceSOUT;
+ dg::SignalPtr<dg::Vector, int> torqueFullSizeSIN;
+ dg::SignalTimeDependent<dg::Vector, int> torqueReduceSOUT;
+ dg::SignalPtr<dg::Vector, int> torqueLimitFullSizeSIN;
+ dg::SignalTimeDependent<dg::Vector, int> torqueLimitReduceSOUT;
/* --- OUTPUTS --- */
- dg::SignalTimeDependent<dg::Vector,int> desiredPositionSOUT;
-
-
- public: /* --- COMMANDLINE --- */
+ dg::SignalTimeDependent<dg::Vector, int> desiredPositionSOUT;
+public: /* --- COMMANDLINE --- */
void initCommands();
- void setOffset(const double & value);
+ void setOffset(const double &value);
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_SOTGRIPPERCONTROL_H__
diff --git a/include/sot/core/integrator-abstract-impl.hh b/include/sot/core/integrator-abstract-impl.hh
index f53651bc..2f4b15bb 100644
--- a/include/sot/core/integrator-abstract-impl.hh
+++ b/include/sot/core/integrator-abstract-impl.hh
@@ -17,14 +17,14 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (integrator_abstract_EXPORTS)
-# define INTEGRATOR_ABSTRACT_EXPORT __declspec(dllexport)
-# else
-# define INTEGRATOR_ABSTRACT_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(integrator_abstract_EXPORTS)
+#define INTEGRATOR_ABSTRACT_EXPORT __declspec(dllexport)
#else
-# define INTEGRATOR_ABSTRACT_EXPORT
+#define INTEGRATOR_ABSTRACT_EXPORT __declspec(dllimport)
+#endif
+#else
+#define INTEGRATOR_ABSTRACT_EXPORT
#endif
/* --------------------------------------------------------------------- */
@@ -32,21 +32,21 @@
/* --------------------------------------------------------------------- */
#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) \
- typedef IntegratorAbstract<sotSigType,sotCoefType> className;
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+ typedef IntegratorAbstract<sotSigType, sotCoefType> className;
#endif
-namespace dynamicgraph { namespace sot {
- DECLARE_SPECIFICATION(IntegratorAbstractDouble,double,double)
- DECLARE_SPECIFICATION(IntegratorAbstractVector,dg::Vector,dg::Matrix)
-}
-}
+namespace dynamicgraph {
+namespace sot {
+DECLARE_SPECIFICATION(IntegratorAbstractDouble, double, double)
+DECLARE_SPECIFICATION(IntegratorAbstractVector, dg::Vector, dg::Matrix)
+} // 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 4662b2b2..b62a6321 100644
--- a/include/sot/core/integrator-abstract.hh
+++ b/include/sot/core/integrator-abstract.hh
@@ -18,12 +18,12 @@
#include <dynamic-graph/linear-algebra.h>
/* SOT */
-#include <sot/core/flags.hh>
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/pool.h>
#include <dynamic-graph/all-signals.h>
#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>
/* STD */
#include <string>
@@ -32,7 +32,8 @@
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*! \brief integrates an ODE. If Y is the output and X the input, the
@@ -40,69 +41,67 @@ namespace dg = dynamicgraph;
* a_p * d(p)Y / dt^p + .... + a_0 Y = b_m * d(m)X / dt^m + ... . b_0 X
* a_i are the coefficients of the denominator of the associated transfer
* function between X and Y, while the b_i are those of the numerator.
-*/
-template<class sigT, class coefT>
-class IntegratorAbstract
-:public dg::Entity
-{
- public:
- IntegratorAbstract ( const std::string& name )
- :dg::Entity(name)
- ,SIN(NULL,"sotIntegratorAbstract("+name+")::input(vector)::sin")
- ,SOUT(boost::bind(&IntegratorAbstract<sigT,coefT>::integrate,this,_1,_2),
- SIN,
- "sotIntegratorAbstract("+name+")::output(vector)::sout")
- {
- signalRegistration( SIN<<SOUT );
+ */
+template <class sigT, class coefT>
+class IntegratorAbstract : public dg::Entity {
+public:
+ IntegratorAbstract(const std::string &name)
+ : dg::Entity(name),
+ SIN(NULL, "sotIntegratorAbstract(" + name + ")::input(vector)::sin"),
+ SOUT(boost::bind(&IntegratorAbstract<sigT, coefT>::integrate, this, _1,
+ _2),
+ SIN, "sotIntegratorAbstract(" + name + ")::output(vector)::sout") {
+ signalRegistration(SIN << SOUT);
using namespace dg::command;
const std::string typeName =
- Value::typeName(dg::command::ValueHelper<coefT>::TypeID);
-
- addCommand ("pushNumCoef",
- makeCommandVoid1 (*this, &IntegratorAbstract::pushNumCoef,
- docCommandVoid1 ("Push a new numerator coefficient", typeName)
- ));
- addCommand ("pushDenomCoef",
- makeCommandVoid1 (*this, &IntegratorAbstract::pushDenomCoef,
- docCommandVoid1 ("Push a new denomicator coefficient", typeName)
- ));
-
- addCommand ("popNumCoef",
- makeCommandVoid0 (*this, &IntegratorAbstract::popNumCoef,
- docCommandVoid0 ("Pop a new numerator coefficient")
- ));
- addCommand ("popDenomCoef",
- makeCommandVoid0 (*this, &IntegratorAbstract::popDenomCoef,
- docCommandVoid0 ("Pop a new denomicator coefficient")
- ));
+ Value::typeName(dg::command::ValueHelper<coefT>::TypeID);
+
+ addCommand(
+ "pushNumCoef",
+ makeCommandVoid1(
+ *this, &IntegratorAbstract::pushNumCoef,
+ docCommandVoid1("Push a new numerator coefficient", typeName)));
+ addCommand(
+ "pushDenomCoef",
+ makeCommandVoid1(
+ *this, &IntegratorAbstract::pushDenomCoef,
+ docCommandVoid1("Push a new denomicator coefficient", typeName)));
+
+ addCommand(
+ "popNumCoef",
+ makeCommandVoid0(*this, &IntegratorAbstract::popNumCoef,
+ docCommandVoid0("Pop a new numerator coefficient")));
+ addCommand(
+ "popDenomCoef",
+ makeCommandVoid0(*this, &IntegratorAbstract::popDenomCoef,
+ docCommandVoid0("Pop a new denomicator coefficient")));
}
virtual ~IntegratorAbstract() {}
- virtual sigT& integrate( sigT& res,int time ) = 0;
+ virtual sigT &integrate(sigT &res, int time) = 0;
- public:
- void pushNumCoef(const coefT& numCoef) { numerator.push_back(numCoef); }
- void pushDenomCoef(const coefT& denomCoef) { denominator.push_back(denomCoef); }
+public:
+ void pushNumCoef(const coefT &numCoef) { numerator.push_back(numCoef); }
+ void pushDenomCoef(const coefT &denomCoef) {
+ denominator.push_back(denomCoef);
+ }
void popNumCoef() { numerator.pop_back(); }
void popDenomCoef() { denominator.pop_back(); }
- public:
+public:
dg::SignalPtr<sigT, int> SIN;
dg::SignalTimeDependent<sigT, int> SOUT;
- protected:
+protected:
std::vector<coefT> numerator;
std::vector<coefT> denominator;
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif
diff --git a/include/sot/core/integrator-euler-impl.hh b/include/sot/core/integrator-euler-impl.hh
index fb6dffbd..ae22a2d0 100644
--- a/include/sot/core/integrator-euler-impl.hh
+++ b/include/sot/core/integrator-euler-impl.hh
@@ -21,35 +21,35 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (integrator_euler_EXPORTS)
-# define INTEGRATOR_EULER_EXPORT __declspec(dllexport)
-# else
-# define INTEGRATOR_EULER_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(integrator_euler_EXPORTS)
+#define INTEGRATOR_EULER_EXPORT __declspec(dllexport)
#else
-# define INTEGRATOR_EULER_EXPORT
+#define INTEGRATOR_EULER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define INTEGRATOR_EULER_EXPORT
#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 ); \
- };
-# else
-# define DECLARE_SPECIFICATION(className, sotSigType,sotCoefType ) \
- typedef IntegratorEuler<sotSigType,sotCoefType> className;
-# endif // WIN32
+#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); \
+ };
+#else
+#define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \
+ typedef IntegratorEuler<sotSigType, sotCoefType> className;
+#endif // WIN32
namespace dynamicgraph {
- namespace sot {
- DECLARE_SPECIFICATION(IntegratorEulerDoubleDouble,double,double)
- DECLARE_SPECIFICATION(IntegratorEulerVectorDouble,Vector,double)
- DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix,Vector,Matrix)
- }
-}
+namespace sot {
+DECLARE_SPECIFICATION(IntegratorEulerDoubleDouble, double, double)
+DECLARE_SPECIFICATION(IntegratorEulerVectorDouble, Vector, double)
+DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix, Vector, Matrix)
+} // namespace sot
+} // namespace dynamicgraph
#endif
diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh
index da35b32e..3122e5b9 100644
--- a/include/sot/core/integrator-euler.hh
+++ b/include/sot/core/integrator-euler.hh
@@ -15,15 +15,16 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/integrator-abstract.hh>
-#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <sot/core/integrator-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
/*!
@@ -34,52 +35,53 @@ namespace dg = dynamicgraph;
* previous values of the other derivatives and the input
* signal, then integrated n times, which will most certainly
* induce a huge drift for ODEs with a high order at the denominator.
-*/
-template<class sigT,class coefT>
-class IntegratorEuler
- : public IntegratorAbstract<sigT,coefT>
-{
-
- public:
- virtual const std::string& getClassName( void ) const;
- static std::string getTypeName( void ) { return "Unknown"; }
+ */
+template <class sigT, class coefT>
+class IntegratorEuler : public IntegratorAbstract<sigT, coefT> {
+
+public:
+ virtual const std::string &getClassName(void) const;
+ static std::string getTypeName(void) { return "Unknown"; }
static const std::string CLASS_NAME;
- public:
- using IntegratorAbstract<sigT,coefT>::SIN;
- using IntegratorAbstract<sigT,coefT>::SOUT;
- using IntegratorAbstract<sigT,coefT>::numerator;
- using IntegratorAbstract<sigT,coefT>::denominator;
-
- public:
- IntegratorEuler( const std::string& name )
- : IntegratorAbstract<sigT,coefT>( name )
- , derivativeSOUT(boost::bind(&IntegratorEuler<sigT,coefT>::derivative,this,_1,_2),
- SOUT,
- "sotIntegratorEuler("+name+")::output(vector)::derivativesout")
- {
- this->signalRegistration( derivativeSOUT );
+public:
+ using IntegratorAbstract<sigT, coefT>::SIN;
+ using IntegratorAbstract<sigT, coefT>::SOUT;
+ using IntegratorAbstract<sigT, coefT>::numerator;
+ using IntegratorAbstract<sigT, coefT>::denominator;
+
+public:
+ IntegratorEuler(const std::string &name)
+ : IntegratorAbstract<sigT, coefT>(name),
+ derivativeSOUT(boost::bind(&IntegratorEuler<sigT, coefT>::derivative,
+ this, _1, _2),
+ SOUT,
+ "sotIntegratorEuler(" + name +
+ ")::output(vector)::derivativesout") {
+ this->signalRegistration(derivativeSOUT);
using namespace dg::command;
- setSamplingPeriod (0.005);
-
- this->addCommand ("setSamplingPeriod",
- new Setter<IntegratorEuler,double> (*this,
- &IntegratorEuler::setSamplingPeriod,
- "Set the time during two sampling."));
- this->addCommand ("getSamplingPeriod",
- new Getter<IntegratorEuler,double> (*this,
- &IntegratorEuler::getSamplingPeriod,
- "Get the time during two sampling."));
-
- this->addCommand ("initialize",
- makeCommandVoid0 (*this, &IntegratorEuler::initialize,
- docCommandVoid0 ("Initialize internal memory from current value of input")
- ));
+ setSamplingPeriod(0.005);
+
+ this->addCommand("setSamplingPeriod",
+ new Setter<IntegratorEuler, double>(
+ *this, &IntegratorEuler::setSamplingPeriod,
+ "Set the time during two sampling."));
+ this->addCommand("getSamplingPeriod",
+ new Getter<IntegratorEuler, double>(
+ *this, &IntegratorEuler::getSamplingPeriod,
+ "Get the time during two sampling."));
+
+ this->addCommand(
+ "initialize",
+ makeCommandVoid0(
+ *this, &IntegratorEuler::initialize,
+ docCommandVoid0(
+ "Initialize internal memory from current value of input")));
}
- virtual ~IntegratorEuler( void ) {}
+ virtual ~IntegratorEuler(void) {}
protected:
std::vector<sigT> inputMemory;
@@ -91,14 +93,13 @@ protected:
double invdt;
public:
- sigT& integrate( sigT& res, int time )
- {
- sotDEBUG(15)<<"# In {"<<std::endl;
+ sigT &integrate(sigT &res, int time) {
+ sotDEBUG(15) << "# In {" << std::endl;
sigT sum;
sigT tmp1, tmp2;
- const std::vector<coefT>& num = numerator;
- const std::vector<coefT>& denom = denominator;
+ const std::vector<coefT> &num = numerator;
+ const std::vector<coefT> &denom = denominator;
// Step 1
tmp1 = inputMemory[0];
@@ -108,9 +109,8 @@ public:
// Step 2
int numsize = (int)num.size();
- for(int i = 1; i < numsize; ++i)
- {
- tmp2 = inputMemory[i-1] - tmp1;
+ for (int i = 1; i < numsize; ++i) {
+ tmp2 = inputMemory[i - 1] - tmp1;
tmp2 *= invdt;
tmp1 = inputMemory[i];
inputMemory[i] = tmp2;
@@ -120,72 +120,61 @@ public:
// Step 3
int denomsize = (int)denom.size() - 1;
- for(int i = 0; i < denomsize; ++i)
- {
+ for (int i = 0; i < denomsize; ++i) {
sum -= (denom[i] * outputMemory[i]);
}
- // End of step 3. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X - a_0 Y - ... a_n-1 d(n-1)Y / dt^(n-1)
+ // End of step 3. Here, sum is b_m * d(m)X / dt^m + ... - b_0 X - a_0 Y -
+ // ... a_n-1 d(n-1)Y / dt^(n-1)
// Step 4
outputMemory[denomsize] = sum;
- for(int i = denomsize-1; i >= 0; --i)
- {
- outputMemory[i] += (outputMemory[i+1] * dt);
+ for (int i = denomsize - 1; i >= 0; --i) {
+ outputMemory[i] += (outputMemory[i + 1] * dt);
}
// End of step 4. The ODE is integrated
inputMemory[0] = SIN.access(time);
res = outputMemory[0];
- sotDEBUG(15)<<"# Out }"<<std::endl;
+ sotDEBUG(15) << "# Out }" << std::endl;
return res;
}
- sigT& derivative ( sigT& res, int time )
- {
+ sigT &derivative(sigT &res, int time) {
if (outputMemory.size() < 2)
- throw dg::ExceptionSignal (dg::ExceptionSignal::GENERIC,
- "Integrator does not compute the derivative.");
+ throw dg::ExceptionSignal(dg::ExceptionSignal::GENERIC,
+ "Integrator does not compute the derivative.");
SOUT.recompute(time);
res = outputMemory[1];
return res;
}
- void setSamplingPeriod (const double& period)
- {
+ void setSamplingPeriod(const double &period) {
dt = period;
- invdt = 1/period;
+ invdt = 1 / period;
}
- double getSamplingPeriod () const
- {
- return dt;
- }
+ double getSamplingPeriod() const { return dt; }
- void initialize ()
- {
+ void initialize() {
std::size_t numsize = numerator.size();
inputMemory.resize(numsize);
inputMemory[0] = SIN.accessCopy();
- for(std::size_t i = 1; i < numsize; ++i)
- {
+ for (std::size_t i = 1; i < numsize; ++i) {
inputMemory[i] = inputMemory[0];
}
std::size_t denomsize = denominator.size();
outputMemory.resize(denomsize);
- for(std::size_t i = 0; i < denomsize; ++i)
- {
+ for (std::size_t i = 0; i < denomsize; ++i) {
outputMemory[i] = inputMemory[0];
}
}
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif
diff --git a/include/sot/core/joint-limitator.hh b/include/sot/core/joint-limitator.hh
index b7c9bbc9..af4d2617 100644
--- a/include/sot/core/joint-limitator.hh
+++ b/include/sot/core/joint-limitator.hh
@@ -8,60 +8,57 @@
*/
#ifndef SOT_FEATURE_JOINTLIMITS_HH
-# define SOT_FEATURE_JOINTLIMITS_HH
+#define SOT_FEATURE_JOINTLIMITS_HH
// Matrix
-# include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
// SOT
-# include <dynamic-graph/entity.h>
-# include <sot/core/exception-task.hh>
-# include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
+#include <sot/core/exception-task.hh>
-#if defined (WIN32)
-# if defined (joint_limitator_EXPORTS)
-# define SOTJOINTLIMITATOR_EXPORT __declspec(dllexport)
-# else
-# define SOTJOINTLIMITATOR_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(joint_limitator_EXPORTS)
+#define SOTJOINTLIMITATOR_EXPORT __declspec(dllexport)
#else
-# define SOTJOINTLIMITATOR_EXPORT
+#define SOTJOINTLIMITATOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTJOINTLIMITATOR_EXPORT
#endif
namespace dynamicgraph {
- namespace sot {
- namespace dg = dynamicgraph;
+namespace sot {
+namespace dg = dynamicgraph;
- /// \brief Filter control vector to avoid exceeding joint maximum values.
- ///
- /// This must be plugged between the entity producing the command
- /// (i.e. usually the sot) and the entity executing it (the device).
- class SOTJOINTLIMITATOR_EXPORT JointLimitator
- : public dg::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL ();
+/// \brief Filter control vector to avoid exceeding joint maximum values.
+///
+/// This must be plugged between the entity producing the command
+/// (i.e. usually the sot) and the entity executing it (the device).
+class SOTJOINTLIMITATOR_EXPORT JointLimitator : public dg::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- JointLimitator (const std::string& name);
- virtual ~JointLimitator ()
- {}
+public:
+ JointLimitator(const std::string &name);
+ virtual ~JointLimitator() {}
- virtual dg::Vector& computeControl (dg::Vector& res, int time);
- dg::Vector& computeWidthJl (dg::Vector& res, const int& time);
+ virtual dg::Vector &computeControl(dg::Vector &res, int time);
+ dg::Vector &computeWidthJl(dg::Vector &res, const int &time);
- virtual void display (std::ostream& os) const;
+ virtual void display(std::ostream &os) const;
- /// \name Signals
- /// \{
- dg::SignalPtr< dg::Vector,int > jointSIN;
- dg::SignalPtr< dg::Vector,int > upperJlSIN;
- dg::SignalPtr< dg::Vector,int > lowerJlSIN;
- dg::SignalPtr< dg::Vector,int > controlSIN;
- dg::SignalTimeDependent< dg::Vector,int > controlSOUT;
- dg::SignalTimeDependent< dg::Vector,int > widthJlSINTERN;
- /// \}
- };
- } // end of namespace sot.
-} // end of namespace dynamic-graph.
+ /// \name Signals
+ /// \{
+ dg::SignalPtr<dg::Vector, int> jointSIN;
+ dg::SignalPtr<dg::Vector, int> upperJlSIN;
+ dg::SignalPtr<dg::Vector, int> lowerJlSIN;
+ dg::SignalPtr<dg::Vector, int> controlSIN;
+ dg::SignalTimeDependent<dg::Vector, int> controlSOUT;
+ dg::SignalTimeDependent<dg::Vector, int> widthJlSINTERN;
+ /// \}
+};
+} // end of namespace sot.
+} // namespace dynamicgraph
#endif //! SOT_FEATURE_JOINTLIMITS_HH
diff --git a/include/sot/core/joint-trajectory-entity.hh b/include/sot/core/joint-trajectory-entity.hh
index ec38e6c0..e435e495 100644
--- a/include/sot/core/joint-trajectory-entity.hh
+++ b/include/sot/core/joint-trajectory-entity.hh
@@ -17,22 +17,22 @@
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
// SOT
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
-#include <sot/core/trajectory.hh>
+#include <dynamic-graph/entity.h>
#include <sot/core/matrix-geometry.hh>
+#include <sot/core/trajectory.hh>
#include <sstream>
// API
-#if defined (WIN32)
-# if defined (joint_trajectory_entity_EXPORTS)
-# define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllexport)
-# else
-# define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(joint_trajectory_entity_EXPORTS)
+#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllexport)
+#else
+#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTJOINT_TRAJECTORY_ENTITY_EXPORT
+#define SOTJOINT_TRAJECTORY_ENTITY_EXPORT
#endif
// Class
@@ -40,51 +40,54 @@ namespace dg = dynamicgraph;
namespace dynamicgraph {
namespace sot {
-/** \brief This object handles trajectory of quantities and publish them as signals.
+/** \brief This object handles trajectory of quantities and publish them as
+ signals.
*/
class SOTJOINT_TRAJECTORY_ENTITY_EXPORT SotJointTrajectoryEntity
- :public dynamicgraph::Entity
-{
+ : public dynamicgraph::Entity {
public:
DYNAMIC_GRAPH_ENTITY_DECL();
-
/// \brief Constructor
- SotJointTrajectoryEntity( const std::string& name );
- virtual ~SotJointTrajectoryEntity( ) { }
+ SotJointTrajectoryEntity(const std::string &name);
+ virtual ~SotJointTrajectoryEntity() {}
- void loadFile( const std::string& name );
+ void loadFile(const std::string &name);
/// \brief Return the next pose for the legs.
- dg::Vector& getNextPosition( dg::Vector& pos, const int& time );
+ dg::Vector &getNextPosition(dg::Vector &pos, const int &time);
/// \brief Return the next com.
- dg::Vector& getNextCoM( dg::Vector& com, const int& time );
+ dg::Vector &getNextCoM(dg::Vector &com, const int &time);
/// \brief Return the next cop.
- dg::Vector& getNextCoP( dg::Vector& cop, const int& time );
+ dg::Vector &getNextCoP(dg::Vector &cop, const int &time);
/// \brief Return the next waist.
- sot::MatrixHomogeneous& getNextWaist(sot::MatrixHomogeneous& waist, const int& time );
+ sot::MatrixHomogeneous &getNextWaist(sot::MatrixHomogeneous &waist,
+ const int &time);
/// \brief Return the current seq identified of the current trajectory.
- unsigned int & getSeqId(unsigned int &seqid, const int& time );
+ unsigned int &getSeqId(unsigned int &seqid, const int &time);
/// \brief Convert a xyztheta vector into an homogeneous matrix
- sot::MatrixHomogeneous XYZThetaToMatrixHomogeneous (const dg::Vector& xyztheta);
+ sot::MatrixHomogeneous
+ XYZThetaToMatrixHomogeneous(const dg::Vector &xyztheta);
/// \brief Perform one update of the signals.
- int & OneStepOfUpdate(int &dummy, const int& time);
+ int &OneStepOfUpdate(int &dummy, const int &time);
/// @name Display
/// @{
- virtual void display( std::ostream& os ) const;
+ virtual void display(std::ostream &os) const;
SOTJOINT_TRAJECTORY_ENTITY_EXPORT
- friend std::ostream& operator<<
- ( std::ostream& os,const SotJointTrajectoryEntity& r )
- { r.display(os); return os;}
+ friend std::ostream &operator<<(std::ostream &os,
+ const SotJointTrajectoryEntity &r) {
+ r.display(os);
+ return os;
+ }
/// @}
public:
@@ -93,32 +96,31 @@ public:
/// @name Signals
/// @{
/// \brief Internal signal for synchronisation.
- dynamicgraph::SignalTimeDependent<int,int> refresherSINTERN;
+ dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
/// \brief Internal signal to trigger one step of the algorithm.
- SignalTimeDependent<Dummy,int> OneStepOfUpdateS;
+ SignalTimeDependent<Dummy, int> OneStepOfUpdateS;
/// \brief Publish pose for each evaluation of the graph.
- dynamicgraph::SignalTimeDependent<dg::Vector,int> positionSOUT;
+ dynamicgraph::SignalTimeDependent<dg::Vector, int> positionSOUT;
/// \brief Publish com for each evaluation of the graph.
- dynamicgraph::SignalTimeDependent<dg::Vector,int> comSOUT;
+ dynamicgraph::SignalTimeDependent<dg::Vector, int> comSOUT;
/// \brief Publish zmp for each evaluation of the graph.
- dynamicgraph::SignalTimeDependent<dg::Vector,int> zmpSOUT;
+ dynamicgraph::SignalTimeDependent<dg::Vector, int> zmpSOUT;
/// \brief Publish waist for each evaluation of the graph.
- dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous,int> waistSOUT;
+ dynamicgraph::SignalTimeDependent<sot::MatrixHomogeneous, int> waistSOUT;
/// \brief Publish ID of the trajectory currently realized.
- dynamicgraph::SignalTimeDependent<unsigned int,int> seqIdSOUT;
+ dynamicgraph::SignalTimeDependent<unsigned int, int> seqIdSOUT;
/// \brief Read a trajectory.
- dynamicgraph::SignalPtr<Trajectory,int> trajectorySIN;
+ dynamicgraph::SignalPtr<Trajectory, int> trajectorySIN;
///@}
protected:
-
/// \brief Index on the point along the trajectory.
std::deque<sot::Trajectory>::size_type index_;
@@ -150,14 +152,12 @@ protected:
void UpdatePoint(const JointTrajectoryPoint &aJTP);
/// \brief Update the entity with the trajectory aTrajectory.
- void UpdateTrajectory(const Trajectory & aTrajectory);
+ void UpdateTrajectory(const Trajectory &aTrajectory);
/// \brief Implements the parsing and the affectation of initial trajectory.
void setInitTraj(const std::string &os);
-
};
-
} /* namespace sot */
} /* namespace dynamicgraph */
diff --git a/include/sot/core/kalman.hh b/include/sot/core/kalman.hh
index d670d00c..5180f3b0 100644
--- a/include/sot/core/kalman.hh
+++ b/include/sot/core/kalman.hh
@@ -16,25 +16,24 @@
/* --- INCLUDE -------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-
+#include <Eigen/LU>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
-#include <Eigen/LU>
#include <sot/core/constraint.hh>
/* -------------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------------ */
/* -------------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (kalman_EXPORTS)
-# define SOT_KALMAN_EXPORT __declspec(dllexport)
-# else
-# define SOT_KALMAN_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(kalman_EXPORTS)
+#define SOT_KALMAN_EXPORT __declspec(dllexport)
+#else
+#define SOT_KALMAN_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOT_KALMAN_EXPORT
+#define SOT_KALMAN_EXPORT
#endif
/* -------------------------------------------------------------------------- */
@@ -42,124 +41,116 @@
/* -------------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
+namespace sot {
-class SOT_KALMAN_EXPORT Kalman
-:public Entity
-{
- public:
+class SOT_KALMAN_EXPORT Kalman : public Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
protected:
- unsigned int size_state ;
- unsigned int size_measure ;
+ 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
-
- 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
+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
- SignalTimeDependent< Matrix,int > gainSINTERN; // K
- SignalTimeDependent< Matrix,int > innovationSINTERN; // S
+ 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
public:
- virtual std::string getDocString () const
- {
- return
- "Implementation of extended Kalman filter \n"
- "\n"
- " Dynamics of the system: \n"
- "\n"
- " x = f (x , u ) + w (state) \n"
- " k k-1 k-1 k-1 \n"
- "\n"
- " y = h (x ) + v (observation)\n"
- " k k k \n"
- "\n"
- " Prediction:\n"
- "\n"
- " ^ ^ \n"
- " x = f (x , u ) (state) \n"
- " k|k-1 k-1|k-1 k-1 \n"
- "\n"
- " T \n"
- " P = F P F + Q (covariance)\n"
- " k|k-1 k-1 k-1|k-1 k-1 \n"
- "\n"
- " with\n"
- " \\ \n"
- " d f ^ \n"
- " F = --- (x , u ) \n"
- " k-1 \\ k-1|k-1 k-1 \n"
- " d x \n"
- "\n"
- " \\ \n"
- " d h ^ \n"
- " H = --- (x ) \n"
- " k \\ k-1|k-1 \n"
- " d x \n"
-
- " Update:\n"
- "\n"
- " ^ \n"
- " z = y - h (x ) (innovation)\n"
- " k k k|k-1 \n"
- " T \n"
- " S = H P H + R (innovation covariance)\n"
- " k k k|k-1 k \n"
- " T -1 \n"
- " K = P H S (Kalman gain)\n"
- " k k|k-1 k k \n"
- " ^ ^ \n"
- " x = x + K z (state) \n"
- " k|k k|k-1 k k \n"
- "\n"
- " P =(I - K H ) P \n"
- " k|k k k k|k-1 \n"
- "\n"
- " Signals\n"
- " - input(vector)::x_pred: state prediction\n"
- " ^\n"
- " - input(vector)::y_pred: observation prediction: h (x )\n"
- " k|k-1\n"
- " - input(matrix)::F: partial derivative wrt x of f\n"
- " - input(vector)::y: measure \n"
- " - input(matrix)::H: partial derivative wrt x of h\n"
- " - input(matrix)::Q: variance of noise w\n"
- " k-1\n"
- " - input(matrix)::R: variance of noise v\n"
- " k\n"
- " - output(matrix)::P_pred: variance of prediction\n"
- " ^\n"
- " - output(vector)::x_est: state estimation x\n"
- " k|k\n";
+ virtual std::string getDocString() const {
+ return "Implementation of extended Kalman filter \n"
+ "\n"
+ " Dynamics of the system: \n"
+ "\n"
+ " x = f (x , u ) + w (state) \n"
+ " k k-1 k-1 k-1 \n"
+ "\n"
+ " y = h (x ) + v (observation)\n"
+ " k k k \n"
+ "\n"
+ " Prediction:\n"
+ "\n"
+ " ^ ^ \n"
+ " x = f (x , u ) (state) \n"
+ " k|k-1 k-1|k-1 k-1 \n"
+ "\n"
+ " T \n"
+ " P = F P F + Q (covariance)\n"
+ " k|k-1 k-1 k-1|k-1 k-1 \n"
+ "\n"
+ " with\n"
+ " \\ \n"
+ " d f ^ \n"
+ " F = --- (x , u ) \n"
+ " k-1 \\ k-1|k-1 k-1 \n"
+ " d x \n"
+ "\n"
+ " \\ \n"
+ " d h ^ \n"
+ " H = --- (x ) \n"
+ " k \\ k-1|k-1 \n"
+ " d x \n"
+
+ " Update:\n"
+ "\n"
+ " ^ \n"
+ " z = y - h (x ) (innovation)\n"
+ " k k k|k-1 \n"
+ " T \n"
+ " S = H P H + R (innovation covariance)\n"
+ " k k k|k-1 k \n"
+ " T -1 \n"
+ " K = P H S (Kalman gain)\n"
+ " k k|k-1 k k \n"
+ " ^ ^ \n"
+ " x = x + K z (state) \n"
+ " k|k k|k-1 k k \n"
+ "\n"
+ " P =(I - K H ) P \n"
+ " k|k k k k|k-1 \n"
+ "\n"
+ " Signals\n"
+ " - input(vector)::x_pred: state prediction\n"
+ " ^\n"
+ " - input(vector)::y_pred: observation prediction: h (x )\n"
+ " k|k-1\n"
+ " - input(matrix)::F: partial derivative wrt x of f\n"
+ " - input(vector)::y: measure \n"
+ " - input(matrix)::H: partial derivative wrt x of h\n"
+ " - input(matrix)::Q: variance of noise w\n"
+ " k-1\n"
+ " - input(matrix)::R: variance of noise v\n"
+ " k\n"
+ " - output(matrix)::P_pred: variance of prediction\n"
+ " ^\n"
+ " - output(vector)::x_est: state estimation x\n"
+ " k|k\n";
}
+
protected:
- Matrix& computeVarianceUpdate (Matrix& P_k_k, const int& time);
- Vector& computeStateUpdate (Vector& x_est,const int& time );
+ Matrix &computeVarianceUpdate(Matrix &P_k_k, const int &time);
+ Vector &computeStateUpdate(Vector &x_est, const int &time);
- void setStateEstimation (const Vector& x0)
- {
+ void setStateEstimation(const Vector &x0) {
stateEstimation_ = x0;
- stateUpdateSOUT.recompute (0);
+ stateUpdateSOUT.recompute(0);
}
- void setStateVariance (const Matrix& P0)
- {
+ void setStateVariance(const Matrix &P0) {
stateVariance_ = P0;
- varianceUpdateSOUT.recompute (0);
+ varianceUpdateSOUT.recompute(0);
}
// Current state estimation
// ^
@@ -190,18 +181,16 @@ protected:
// Kalman Gain
Matrix K_;
+
public:
- Kalman( const std::string & name ) ;
+ Kalman(const std::string &name);
/* --- Entity --- */
- void display( std::ostream& os ) const;
-} ;
+ void display(std::ostream &os) const;
+};
-
- } // namespace sot
+} // namespace sot
} // namespace dynamicgraph
-
-
/*!
\file Kalman.h
\brief Extended kalman filter implementation
diff --git a/include/sot/core/latch.hh b/include/sot/core/latch.hh
index b2c903e8..fc864a6c 100644
--- a/include/sot/core/latch.hh
+++ b/include/sot/core/latch.hh
@@ -11,69 +11,72 @@
/* --------------------------------------------------------------------- */
/* SOT */
+#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/command-bind.h>
-#include <sot/core/pool.hh>
#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
+#include <sot/core/pool.hh>
/* STD */
#include <string>
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
- using dynamicgraph::Entity;
- using dynamicgraph::command::makeCommandVoid0;
- using dynamicgraph::command::docCommandVoid0;
-
- class Latch : public Entity
- {
-
- public: /* --- SIGNAL --- */
- DYNAMIC_GRAPH_ENTITY_DECL();
- Signal<bool,int> outSOUT;
- Signal<bool,int> turnOnSOUT;
- Signal<bool,int> turnOffSOUT;
-
- protected:
- bool signalOutput;
- void turnOn(){ signalOutput = true; }
- bool& turnOnLatch(bool& res, int){ res = signalOutput = true; return res; }
-
- void turnOff(){ signalOutput = false; }
- bool& turnOffLatch(bool& res, int){ res = signalOutput = false; return res; }
-
- bool& latchOutput(bool& res, int){ res = signalOutput; return res; }
-
- public:
- Latch( const std::string& name )
- : 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));
- turnOnSOUT .setFunction(boost::bind(&Latch::turnOnLatch ,this,_1,_2));
- turnOffSOUT.setFunction(boost::bind(&Latch::turnOffLatch,this,_1,_2));
- signalOutput = false;
- signalRegistration (outSOUT << turnOnSOUT << turnOffSOUT);
- addCommand ("turnOn",
- makeCommandVoid0 (*this, &Latch::turnOn,
- docCommandVoid0 ("Turn on the latch")));
- addCommand ("turnOff",
- makeCommandVoid0 (*this, &Latch::turnOff,
- docCommandVoid0 ("Turn off the latch")));
- }
-
- virtual ~Latch( void ) {};
+namespace sot {
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+using dynamicgraph::Entity;
+using dynamicgraph::command::docCommandVoid0;
+using dynamicgraph::command::makeCommandVoid0;
+
+class Latch : public Entity {
+
+public: /* --- SIGNAL --- */
+ DYNAMIC_GRAPH_ENTITY_DECL();
+ Signal<bool, int> outSOUT;
+ Signal<bool, int> turnOnSOUT;
+ Signal<bool, int> turnOffSOUT;
+
+protected:
+ bool signalOutput;
+ void turnOn() { signalOutput = true; }
+ bool &turnOnLatch(bool &res, int) {
+ res = signalOutput = true;
+ return res;
+ }
+
+ void turnOff() { signalOutput = false; }
+ bool &turnOffLatch(bool &res, int) {
+ res = signalOutput = false;
+ return res;
+ }
+
+ bool &latchOutput(bool &res, int) {
+ res = signalOutput;
+ return res;
+ }
+
+public:
+ Latch(const std::string &name)
+ : 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));
+ turnOnSOUT.setFunction(boost::bind(&Latch::turnOnLatch, this, _1, _2));
+ turnOffSOUT.setFunction(boost::bind(&Latch::turnOffLatch, this, _1, _2));
+ signalOutput = false;
+ signalRegistration(outSOUT << turnOnSOUT << turnOffSOUT);
+ addCommand("turnOn",
+ makeCommandVoid0(*this, &Latch::turnOn,
+ docCommandVoid0("Turn on the latch")));
+ addCommand("turnOff",
+ makeCommandVoid0(*this, &Latch::turnOff,
+ docCommandVoid0("Turn off the latch")));
+ }
+
+ virtual ~Latch(void){};
};
} /* namespace sot */
} /* namespace dynamicgraph */
-
-
#endif // #ifndef __SOT_LATCH_H__
diff --git a/include/sot/core/macros-signal.hh b/include/sot/core/macros-signal.hh
index d95eed1e..3b2e182f 100644
--- a/include/sot/core/macros-signal.hh
+++ b/include/sot/core/macros-signal.hh
@@ -11,156 +11,111 @@
/* --- GENERIC MACROS ------------------------------------------------------- */
/* --- GENERIC MACROS ------------------------------------------------------- */
-#define SOT_CALL_SIG(sotName,sotType) \
- boost::bind(&Signal<sotType,int>::access, \
- &sotName,_2)
+#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) \
-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), \
- 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), \
- 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), \
- 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), \
- 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), \
- 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
+#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), \
+ 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), \
+ 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), \
+ 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), \
+ 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), \
+ 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), \
- 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), \
- 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), \
- 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), \
- 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), \
- sotArg1<<sotArg2<<sotArg3<<sotArg4<<sotArg5<<sotArg6
+#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), \
+ 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), \
+ 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), \
+ 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), \
+ sotArg1 << sotArg2 << sotArg3 << sotArg4 << sotArg5 << sotArg6
diff --git a/include/sot/core/madgwickahrs.hh b/include/sot/core/madgwickahrs.hh
index 81b426bb..a80021db 100644
--- a/include/sot/core/madgwickahrs.hh
+++ b/include/sot/core/madgwickahrs.hh
@@ -34,101 +34,98 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (madgwickahrs_EXPORTS)
-# define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
-# else
-# define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(madgwickahrs_EXPORTS)
+#define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
#else
-# define SOTMADGWICKAHRS_EXPORT
+#define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTMADGWICKAHRS_EXPORT
#endif
-
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+#include "boost/assign.hpp"
#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
#include <map>
-#include "boost/assign.hpp"
+#include <sot/core/matrix-geometry.hh>
#define betaDef 0.01 // 2 * proportional g
namespace dynamicgraph {
- namespace sot {
- /** \addtogroup Filters
- \section subsec_madgwickahrs MadgwickAHRS filter
- \class MadgwickARHS
- This class implements the MadgwickAHRS filter as described
- in http://x-io.co.uk/res/doc/madgwick_internal_report.pdf
- This method uses a gradient descent approach to compute the orientation
- from an IMU.
-
- The signals input are:
- <ul>
- <li>m_accelerometerSIN: \f$[a_x, a_y, a_z]^T\f$ in \f$m.s^{-2}\f$</li>
- <li>m_gyroscopeSIN: \f$[g_x, g_y, g_z]^T\f$ in \f$rad.s^{-1}\f$</li>
- <li>m_imu_quatSOUT: \f$[q_0, q_1, q_2, q_3]^T\f$ </li> estimated rotation
- as a quaternion</li>
- </ul>
-
- The internal parameters are:
- <ul>
- <li>\f$Beta\f$: Gradient step weight (default to 0.01) </li>
- <li>\f$m_sampleFref\f$: Sampling Frequency computed from the control
- period when using init.</li>
- </ul>
- */
- class SOTMADGWICKAHRS_EXPORT MadgwickAHRS
- :public::dynamicgraph::Entity
- {
- typedef MadgwickAHRS EntityClassName;
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /* --- CONSTRUCTOR ---- */
- MadgwickAHRS( const std::string & name );
-
- void init(const double& dt);
- void set_beta(const double & beta);
-
- /// Set the quaternion as [w,x,y,z]
- void set_imu_quat(const dynamicgraph::Vector & imu_quat);
-
- /* --- SIGNALS --- */
- /// ax ay az in m.s-2
- DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
- /// gx gy gz in rad.s-1
- DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
- /// Estimated orientation of IMU as a quaternion
- DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector);
-
- protected:
- /* --- COMMANDS --- */
- /* --- ENTITY INHERITANCE --- */
- virtual void display( std::ostream& os ) const;
-
- /* --- METHODS --- */
- double invSqrt(double x);
- void madgwickAHRSupdateIMU
- (double gx, double gy, double gz, double ax, double ay, double az);
-
- protected:
- /// true if the entity has been successfully initialized
- bool m_initSucceeded;
- /// 2 * proportional gain (Kp)
- double m_beta;
- /// quaternion of sensor frame
- double m_q0, m_q1, m_q2, m_q3;
- /// sample frequency in Hz
- double m_sampleFreq;
-
- }; // class MadgwickAHRS
- } // namespace sot
-} // namespace dynamicgraph
+namespace sot {
+/** \addtogroup Filters
+\section subsec_madgwickahrs MadgwickAHRS filter
+\class MadgwickARHS
+This class implements the MadgwickAHRS filter as described
+in http://x-io.co.uk/res/doc/madgwick_internal_report.pdf
+This method uses a gradient descent approach to compute the orientation
+from an IMU.
+
+The signals input are:
+<ul>
+<li>m_accelerometerSIN: \f$[a_x, a_y, a_z]^T\f$ in \f$m.s^{-2}\f$</li>
+<li>m_gyroscopeSIN: \f$[g_x, g_y, g_z]^T\f$ in \f$rad.s^{-1}\f$</li>
+<li>m_imu_quatSOUT: \f$[q_0, q_1, q_2, q_3]^T\f$ </li> estimated rotation
+as a quaternion</li>
+</ul>
+
+The internal parameters are:
+<ul>
+<li>\f$Beta\f$: Gradient step weight (default to 0.01) </li>
+<li>\f$m_sampleFref\f$: Sampling Frequency computed from the control
+period when using init.</li>
+</ul>
+*/
+class SOTMADGWICKAHRS_EXPORT MadgwickAHRS : public ::dynamicgraph::Entity {
+ typedef MadgwickAHRS EntityClassName;
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /* --- CONSTRUCTOR ---- */
+ MadgwickAHRS(const std::string &name);
+
+ void init(const double &dt);
+ void set_beta(const double &beta);
+
+ /// Set the quaternion as [w,x,y,z]
+ void set_imu_quat(const dynamicgraph::Vector &imu_quat);
+
+ /* --- SIGNALS --- */
+ /// ax ay az in m.s-2
+ DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
+ /// gx gy gz in rad.s-1
+ DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
+ /// Estimated orientation of IMU as a quaternion
+ DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector);
+
+protected:
+ /* --- COMMANDS --- */
+ /* --- ENTITY INHERITANCE --- */
+ virtual void display(std::ostream &os) const;
+
+ /* --- METHODS --- */
+ double invSqrt(double x);
+ void madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax,
+ double ay, double az);
+
+protected:
+ /// true if the entity has been successfully initialized
+ bool m_initSucceeded;
+ /// 2 * proportional gain (Kp)
+ double m_beta;
+ /// quaternion of sensor frame
+ double m_q0, m_q1, m_q2, m_q3;
+ /// sample frequency in Hz
+ double m_sampleFreq;
+
+}; // class MadgwickAHRS
+} // namespace sot
+} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_madgwickahrs_H__
diff --git a/include/sot/core/mailbox-vector.hh b/include/sot/core/mailbox-vector.hh
index 53810025..f1ee0ff7 100644
--- a/include/sot/core/mailbox-vector.hh
+++ b/include/sot/core/mailbox-vector.hh
@@ -19,14 +19,14 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (mailbox_vector_EXPORTS)
-# define MAILBOX_VECTOR_EXPORT __declspec(dllexport)
-# else
-# define MAILBOX_VECTOR_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(mailbox_vector_EXPORTS)
+#define MAILBOX_VECTOR_EXPORT __declspec(dllexport)
#else
-# define MAILBOX_VECTOR_EXPORT
+#define MAILBOX_VECTOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define MAILBOX_VECTOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
@@ -34,17 +34,17 @@
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
+namespace sot {
#ifdef WIN32
- class MAILBOX_VECTOR_EXPORT MailboxVector : public Mailbox<dynamicgraph::Vector>
- {
- public:
- MailboxVector( const std::string& name );
- };
+class MAILBOX_VECTOR_EXPORT MailboxVector
+ : public Mailbox<dynamicgraph::Vector> {
+public:
+ MailboxVector(const std::string &name);
+};
#else
- typedef Mailbox<dynamicgraph::Vector> MailboxVector;
+typedef Mailbox<dynamicgraph::Vector> MailboxVector;
#endif
- } // namespace sot
+} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __SOT_MAILBOX_HH
diff --git a/include/sot/core/mailbox.hh b/include/sot/core/mailbox.hh
index 9678bfb0..d28b4b2b 100644
--- a/include/sot/core/mailbox.hh
+++ b/include/sot/core/mailbox.hh
@@ -10,10 +10,9 @@
#ifndef __SOT_MAILBOX_HH
#define __SOT_MAILBOX_HH
-
/* --- SOT PLUGIN --- */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
/* --- BOOST --- */
#include <boost/thread/mutex.hpp>
@@ -29,55 +28,47 @@
#endif /*WIN32*/
#include <string>
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-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; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- public:
- struct sotTimestampedObject
- {
+public:
+ struct sotTimestampedObject {
Object obj;
struct timeval timestamp;
};
public:
+ Mailbox(const std::string &name);
+ ~Mailbox(void);
- Mailbox( const std::string& name );
- ~Mailbox( void );
-
- void post( const Object& obj );
- sotTimestampedObject& get( sotTimestampedObject& res,const int& dummy );
+ void post(const Object &obj);
+ sotTimestampedObject &get(sotTimestampedObject &res, const int &dummy);
- Object& getObject( Object& res,const int& time );
- struct timeval& getTimestamp( struct timeval& res,const int& time );
+ Object &getObject(Object &res, const int &time);
+ struct timeval &getTimestamp(struct timeval &res, const int &time);
- bool hasBeenUpdated( void );
+ bool hasBeenUpdated(void);
protected:
-
boost::timed_mutex mainObjectMutex;
Object mainObject;
struct timeval mainTimeStamp;
bool update;
- public: /* --- SIGNALS --- */
-
- dg::SignalTimeDependent< struct sotTimestampedObject,int > SOUT;
- dg::SignalTimeDependent< Object,int > objSOUT;
- dg::SignalTimeDependent< struct timeval,int > timeSOUT;
-
+public: /* --- SIGNALS --- */
+ dg::SignalTimeDependent<struct sotTimestampedObject, int> SOUT;
+ dg::SignalTimeDependent<Object, int> objSOUT;
+ dg::SignalTimeDependent<struct timeval, int> timeSOUT;
};
-
-
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_MAILBOX_HH
diff --git a/include/sot/core/mailbox.hxx b/include/sot/core/mailbox.hxx
index 035a8e1c..74757585 100644
--- a/include/sot/core/mailbox.hxx
+++ b/include/sot/core/mailbox.hxx
@@ -13,129 +13,106 @@
#include <sot/core/mailbox.hh>
namespace dynamicgraph {
- namespace sot {
-
- namespace dg = dynamicgraph;
-
- /* -------------------------------------------------------------------------- */
- /* --- CONSTRUCTION --------------------------------------------------------- */
- /* -------------------------------------------------------------------------- */
- template< class Object >
- Mailbox<Object>::
- Mailbox( const std::string& name )
- :Entity(name)
- ,mainObjectMutex()
- ,mainObject()
- ,update(false)
-
- ,SOUT( boost::bind(&Mailbox::get,this,_1,_2),
- sotNOSIGNAL,
- "Mailbox("+name+")::output(Object)::sout" )
- ,objSOUT( boost::bind(&Mailbox::getObject,this,_1,_2),
- SOUT,
- "Mailbox("+name+")::output(Object)::object" )
- ,timeSOUT( boost::bind(&Mailbox::getTimestamp,this,_1,_2),
- SOUT,
- "Mailbox("+name+")::output(Object)::timestamp" )
- {
- signalRegistration( SOUT<<objSOUT<<timeSOUT );
- SOUT.setDependencyType( TimeDependency<int>::BOOL_DEPENDENT );
- }
-
- template< class Object >
- Mailbox<Object>::
- ~Mailbox( void )
- {
- boost::timed_mutex::scoped_lock lockMain( mainObjectMutex );
- }
-
- /* -------------------------------------------------------------------------- */
- /* --- ACCESS --------------------------------------------------------------- */
- /* -------------------------------------------------------------------------- */
- template< class Object >
- bool Mailbox<Object>::
- hasBeenUpdated( void )
- {
- boost::timed_mutex::scoped_try_lock lockMain( this->mainObjectMutex );
-
- if( lockMain.owns_lock() )
- {
- return update;
- }
- else
- {
- return false;
- }
- }
-
-
- /* -------------------------------------------------------------------------- */
- template< class Object >
- 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() )
- {
- res.timestamp.tv_sec=this->mainTimeStamp.tv_sec;
- res.timestamp.tv_usec=this->mainTimeStamp.tv_usec;
-
- update = false;
- res.obj = this->mainObject;
- }
-
- return res;
- }
-
- /* -------------------------------------------------------------------------- */
- template< class Object >
- void Mailbox<Object>::
- post( const Object& value )
- {
- boost::timed_mutex::scoped_lock lockMain( this->mainObjectMutex );
- mainObject = value;
- gettimeofday( &this->mainTimeStamp, NULL );
- update = true;
- SOUT.setReady();
-
- return;
- }
-
- template< class Object >
- Object& Mailbox<Object>::
- getObject( Object& res,const int& time )
- {
- const sotTimestampedObject & data = SOUT(time);
- res = data.obj; return res;
- }
-
- template< class Object >
- timeval& Mailbox<Object>::
- getTimestamp( struct timeval& res,const int& time )
- {
- const sotTimestampedObject & data = SOUT(time);
- res.tv_sec = data.timestamp.tv_sec ;
- res.tv_usec = data.timestamp.tv_usec ;
- return res;
- }
-
- } /* namespace sot */
+namespace sot {
+
+namespace dg = dynamicgraph;
+
+/* -------------------------------------------------------------------------- */
+/* --- CONSTRUCTION --------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+template <class Object>
+Mailbox<Object>::Mailbox(const std::string &name)
+ : Entity(name), mainObjectMutex(), mainObject(), update(false)
+
+ ,
+ SOUT(boost::bind(&Mailbox::get, this, _1, _2), sotNOSIGNAL,
+ "Mailbox(" + name + ")::output(Object)::sout"),
+ objSOUT(boost::bind(&Mailbox::getObject, this, _1, _2), SOUT,
+ "Mailbox(" + name + ")::output(Object)::object"),
+ timeSOUT(boost::bind(&Mailbox::getTimestamp, this, _1, _2), SOUT,
+ "Mailbox(" + name + ")::output(Object)::timestamp") {
+ signalRegistration(SOUT << objSOUT << timeSOUT);
+ SOUT.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
+}
+
+template <class Object> Mailbox<Object>::~Mailbox(void) {
+ boost::timed_mutex::scoped_lock lockMain(mainObjectMutex);
+}
+
+/* -------------------------------------------------------------------------- */
+/* --- ACCESS --------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+template <class Object> bool Mailbox<Object>::hasBeenUpdated(void) {
+ boost::timed_mutex::scoped_try_lock lockMain(this->mainObjectMutex);
+
+ if (lockMain.owns_lock()) {
+ return update;
+ } else {
+ return false;
+ }
+}
+
+/* -------------------------------------------------------------------------- */
+template <class Object>
+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()) {
+ res.timestamp.tv_sec = this->mainTimeStamp.tv_sec;
+ res.timestamp.tv_usec = this->mainTimeStamp.tv_usec;
+
+ update = false;
+ res.obj = this->mainObject;
+ }
+
+ return res;
+}
+
+/* -------------------------------------------------------------------------- */
+template <class Object> void Mailbox<Object>::post(const Object &value) {
+ boost::timed_mutex::scoped_lock lockMain(this->mainObjectMutex);
+ mainObject = value;
+ gettimeofday(&this->mainTimeStamp, NULL);
+ update = true;
+ SOUT.setReady();
+
+ return;
+}
+
+template <class Object>
+Object &Mailbox<Object>::getObject(Object &res, const int &time) {
+ const sotTimestampedObject &data = SOUT(time);
+ res = data.obj;
+ return res;
+}
+
+template <class Object>
+timeval &Mailbox<Object>::getTimestamp(struct timeval &res, const int &time) {
+ const sotTimestampedObject &data = SOUT(time);
+ res.tv_sec = data.timestamp.tv_sec;
+ res.tv_usec = data.timestamp.tv_usec;
+ return res;
+}
+
+} /* namespace sot */
} /* 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
+#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 25b5973d..29ba4eaa 100644
--- a/include/sot/core/matrix-constant.hh
+++ b/include/sot/core/matrix-constant.hh
@@ -7,8 +7,8 @@
*
*/
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
@@ -19,35 +19,32 @@ namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
- namespace command {
- namespace matrixConstant {
- class Resize;
- }
- }
-
+namespace sot {
+namespace command {
+namespace matrixConstant {
+class Resize;
+}
+} // namespace command
- class MatrixConstant
- : public Entity
- {
- friend class command::matrixConstant::Resize;
- public:
- static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+class MatrixConstant : public Entity {
+ friend class command::matrixConstant::Resize;
- int rows,cols;
- double color;
+public:
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- void setValue(const dg::Matrix& inValue);
+ int rows, cols;
+ double color;
- public:
- MatrixConstant( const std::string& name );
+ void setValue(const dg::Matrix &inValue);
- virtual ~MatrixConstant( void ){}
+public:
+ MatrixConstant(const std::string &name);
- SignalTimeDependent<dg::Matrix,int> SOUT;
+ virtual ~MatrixConstant(void) {}
- };
+ SignalTimeDependent<dg::Matrix, int> SOUT;
+};
- } // namespace sot
+} // namespace sot
} // namespace dynamicgraph
diff --git a/include/sot/core/matrix-geometry.hh b/include/sot/core/matrix-geometry.hh
index bbe9a303..52046447 100644
--- a/include/sot/core/matrix-geometry.hh
+++ b/include/sot/core/matrix-geometry.hh
@@ -8,100 +8,98 @@
#ifndef __SOT_MATRIX_GEOMETRY_H__
#define __SOT_MATRIX_GEOMETRY_H__
-
/* --- Matrix --- */
-#include <sot/core/api.hh>
-#include <Eigen/Geometry>
#include <Eigen/Core>
-#include <dynamic-graph/linear-algebra.h>
+#include <Eigen/Geometry>
#include <dynamic-graph/eigen-io.h>
+#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/api.hh>
-# define MRAWDATA(x) x.data()
+#define MRAWDATA(x) x.data()
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
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 */ \
- 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 */ \
- 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) \
- EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
-
- EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
- EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
- EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
- EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
- EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
-
- #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
- #undef EIGEN_MAKE_TYPEDEFS
-
- typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> MatrixRXd;
- typedef Eigen::Map<MatrixRXd> SigMatrixXd;
- typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
- typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
- typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;
-
- typedef Eigen::Ref<Eigen::VectorXd> RefVector;
- typedef const Eigen::Ref<const Eigen::VectorXd>& ConstRefVector;
- typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
- typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
-
-
- typedef Eigen::Transform<double,3, Eigen::Affine> SOT_CORE_EXPORT MatrixHomogeneous;
- typedef Eigen::Matrix<double,3,3> SOT_CORE_EXPORT MatrixRotation;
- typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
- typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
- typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
- typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
- typedef Eigen::Matrix<double,6,6> SOT_CORE_EXPORT MatrixForce;
- typedef Eigen::Matrix<double,6,6> SOT_CORE_EXPORT MatrixTwist;
-
- typedef Eigen::Matrix<double,7,1> SOT_CORE_EXPORT Vector7;
- 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;
- Tx << 0, -_t(2), _t(1),
- _t(2), 0, -_t(0),
- -_t(1), _t(0), 0;
- Eigen::Matrix3d sk; sk = Tx*R;
-
- MT.block<3,3>(0,0) = R;
- MT.block<3,3>(0,3) = sk;
- MT.block<3,3>(3,0).setZero();
- MT.block<3,3>(3,3) = R;
- }
-
- } // namespace sot
+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 */ \
+ 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 */ \
+ 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) \
+ EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
+
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
+EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
+
+#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
+#undef EIGEN_MAKE_TYPEDEFS
+
+typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
+ MatrixRXd;
+typedef Eigen::Map<MatrixRXd> SigMatrixXd;
+typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
+typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
+typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;
+
+typedef Eigen::Ref<Eigen::VectorXd> RefVector;
+typedef const Eigen::Ref<const Eigen::VectorXd> &ConstRefVector;
+typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
+typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
+
+typedef Eigen::Transform<double, 3, Eigen::Affine> SOT_CORE_EXPORT
+ MatrixHomogeneous;
+typedef Eigen::Matrix<double, 3, 3> SOT_CORE_EXPORT MatrixRotation;
+typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
+typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
+typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
+typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
+typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixForce;
+typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixTwist;
+
+typedef Eigen::Matrix<double, 7, 1> SOT_CORE_EXPORT Vector7;
+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;
+ Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
+ Eigen::Matrix3d sk;
+ sk = Tx * R;
+
+ MT.block<3, 3>(0, 0) = R;
+ MT.block<3, 3>(0, 3) = sk;
+ MT.block<3, 3>(3, 0).setZero();
+ MT.block<3, 3>(3, 3) = R;
+}
+
+} // 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 a6d214ca..435edd24 100644
--- a/include/sot/core/matrix-svd.hh
+++ b/include/sot/core/matrix-svd.hh
@@ -10,37 +10,29 @@
#ifndef __SOT_MATRIX_SVD_H__
#define __SOT_MATRIX_SVD_H__
-
/* --- Matrix --- */
#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>
-
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace Eigen {
-void pseudoInverse( dg::Matrix& _inputMatrix,
- dg::Matrix& _inverseMatrix,
- const double threshold = 1e-6);
+void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
+ const double threshold = 1e-6);
-void dampedInverse( const JacobiSVD <dg::Matrix>& svd,
- dg::Matrix& _inverseMatrix,
- const double threshold = 1e-6);
+void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
+ const double threshold = 1e-6);
-void dampedInverse( const dg::Matrix& _inputMatrix,
- dg::Matrix& _inverseMatrix,
- dg::Matrix& Uref,
- dg::Vector& Sref,
- dg::Matrix& Vref,
- const double threshold = 1e-6);
+void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
+ dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
+ const double threshold = 1e-6);
-void dampedInverse( const dg::Matrix& _inputMatrix,
- dg::Matrix& _inverseMatrix,
- const double threshold = 1e-6);
+void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
+ const double threshold = 1e-6);
-}
+} // namespace Eigen
#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 497f8a91..ecbe51a8 100644
--- a/include/sot/core/memory-task-sot.hh
+++ b/include/sot/core/memory-task-sot.hh
@@ -10,62 +10,59 @@
#ifndef __SOT_MEMORY_TASK_HH
#define __SOT_MEMORY_TASK_HH
-
-#include <sot/core/task-abstract.hh>
#include "sot/core/api.hh"
+#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
- namespace dg = dynamicgraph;
+namespace sot {
+namespace dg = dynamicgraph;
- class SOT_CORE_EXPORT MemoryTaskSOT
- : public TaskAbstract::MemoryTaskAbstract, public dg::Entity
- {
- public:// protected:
- /* Internal memory to reduce the dynamic allocation at task resolution. */
- dg::Vector err;
- dg::Matrix Jt; //( nJ,mJ );
- dg::Matrix Jp;
- dg::Matrix PJp;
+class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract,
+ public dg::Entity {
+public: // protected:
+ /* Internal memory to reduce the dynamic allocation at task resolution. */
+ dg::Vector err;
+ dg::Matrix Jt; //( nJ,mJ );
+ dg::Matrix Jp;
+ dg::Matrix PJp;
- dg::Matrix Jff; //( nJ,FF_SIZE ); // Free-floating part
- dg::Matrix Jact; //( nJ,mJ ); // Activated part
- dg::Matrix JK; //(nJ,mJ);
+ dg::Matrix Jff; //( nJ,FF_SIZE ); // Free-floating part
+ dg::Matrix Jact; //( nJ,mJ ); // Activated part
+ dg::Matrix JK; //(nJ,mJ);
- dg::Matrix Proj;
+ dg::Matrix Proj;
- typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
- SVD_t svd;
+ typedef Eigen::JacobiSVD<dg::Matrix> SVD_t;
+ SVD_t svd;
- public:
- /* mJ is the number of actuated joints, nJ the number of feature in the task,
- * and ffsize the number of unactuated DOF. */
- MemoryTaskSOT( const std::string & name,const Matrix::Index nJ=0,
- const Matrix::Index mJ=0,const Matrix::Index ffsize =0 );
+public:
+ /* mJ is the number of actuated joints, nJ the number of feature in the task,
+ * and ffsize the number of unactuated DOF. */
+ MemoryTaskSOT(const std::string &name, const Matrix::Index nJ = 0,
+ const Matrix::Index mJ = 0, const Matrix::Index ffsize = 0);
- virtual void initMemory( const Matrix::Index nJ,
- const Matrix::Index mJ,
- const Matrix::Index ffsize,
- bool atConstruction = false);
+ virtual void initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
+ const Matrix::Index ffsize,
+ bool atConstruction = false);
- 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: /* --- 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 --- */
- dg::Signal< dg::Matrix,int > jacobianInvSINOUT;
- dg::Signal< dg::Matrix,int > jacobianConstrainedSINOUT;
- dg::Signal< dg::Matrix,int > jacobianProjectedSINOUT;
- dg::Signal< dg::Matrix,int > singularBaseImageSINOUT;
- dg::Signal< unsigned int,int > rankSINOUT;
- };
+public: /* --- SIGNALS --- */
+ dg::Signal<dg::Matrix, int> jacobianInvSINOUT;
+ dg::Signal<dg::Matrix, int> jacobianConstrainedSINOUT;
+ dg::Signal<dg::Matrix, int> jacobianProjectedSINOUT;
+ dg::Signal<dg::Matrix, int> singularBaseImageSINOUT;
+ dg::Signal<unsigned int, int> rankSINOUT;
+};
- } /* namespace sot */
+} /* namespace sot */
} /* namespace dynamicgraph */
#endif // __SOT_MEMORY_TASK_HH
diff --git a/include/sot/core/motion-period.hh b/include/sot/core/motion-period.hh
index 889775db..b2fc894d 100644
--- a/include/sot/core/motion-period.hh
+++ b/include/sot/core/motion-period.hh
@@ -19,55 +19,46 @@
namespace dg = dynamicgraph;
/* SOT */
+#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/exception-task.hh>
-#include <dynamic-graph/all-signals.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (motion_period_EXPORTS)
-# define SOTMOTIONPERIOD_EXPORT __declspec(dllexport)
-# else
-# define SOTMOTIONPERIOD_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(motion_period_EXPORTS)
+#define SOTMOTIONPERIOD_EXPORT __declspec(dllexport)
+#else
+#define SOTMOTIONPERIOD_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTMOTIONPERIOD_EXPORT
+#define SOTMOTIONPERIOD_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/*!
\class MotionPeriod
*/
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTMOTIONPERIOD_EXPORT MotionPeriod
-: public dg::Entity
-{
+class SOTMOTIONPERIOD_EXPORT MotionPeriod : public dg::Entity {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- enum MotionPeriodType
- {
- MOTION_CONSTANT
- ,MOTION_SIN
- ,MOTION_COS
- };
+protected:
+ enum MotionPeriodType { MOTION_CONSTANT, MOTION_SIN, MOTION_COS };
- struct sotMotionParam
- {
+ struct sotMotionParam {
MotionPeriodType motionType;
unsigned int period;
unsigned int initPeriod;
@@ -76,26 +67,25 @@ class SOTMOTIONPERIOD_EXPORT MotionPeriod
};
unsigned int size;
- std::vector< sotMotionParam > motionParams;
-
- void resize( const unsigned int & size );
+ std::vector<sotMotionParam> motionParams;
+ void resize(const unsigned int &size);
/* --- SIGNALS ------------------------------------------------------------ */
- public:
-
- dg::SignalTimeDependent< dg::Vector,int > motionSOUT;
+public:
+ dg::SignalTimeDependent<dg::Vector, int> motionSOUT;
- public:
- MotionPeriod( const std::string& name );
- virtual ~MotionPeriod( void ) {}
+public:
+ MotionPeriod(const std::string &name);
+ virtual ~MotionPeriod(void) {}
- dg::Vector& computeMotion( dg::Vector& res,const int& time );
+ dg::Vector &computeMotion(dg::Vector &res, const int &time);
- virtual void display( std::ostream& os ) const;
-} ;
+ virtual void display(std::ostream &os) const;
+};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_JOINTLIMITS_HH__
diff --git a/include/sot/core/multi-bound.hh b/include/sot/core/multi-bound.hh
index 2b6fcf0a..f3ee3098 100644
--- a/include/sot/core/multi-bound.hh
+++ b/include/sot/core/multi-bound.hh
@@ -26,48 +26,51 @@
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
-class SOT_CORE_EXPORT MultiBound
-{
- public:
+class SOT_CORE_EXPORT MultiBound {
+public:
enum MultiBoundModeType { MODE_SINGLE, MODE_DOUBLE };
- enum SupInfType { BOUND_SUP,BOUND_INF };
+ enum SupInfType { BOUND_SUP, BOUND_INF };
- public:// protected:
+public: // protected:
MultiBoundModeType mode;
double boundSingle;
- double boundSup,boundInf;
- bool boundSupSetup,boundInfSetup;
-
- 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
- MultiBoundModeType getMode( void ) const;
- double getSingleBound( void ) const;
- double getDoubleBound( const SupInfType bound ) const;
- bool getDoubleBoundSetup( const SupInfType bound ) const;
-
- public: // Modifiors
- void setDoubleBound( SupInfType boundType,double boundValue );
- void unsetDoubleBound( SupInfType boundType );
- void setSingleBound( double boundValue );
-
- public:
- SOT_CORE_EXPORT friend std::ostream& operator<< ( std::ostream& os, const MultiBound & m );
- SOT_CORE_EXPORT friend std::istream& operator>> ( std::istream& is, MultiBound & m );
+ double boundSup, boundInf;
+ bool boundSupSetup, boundInfSetup;
+
+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
+ MultiBoundModeType getMode(void) const;
+ double getSingleBound(void) const;
+ double getDoubleBound(const SupInfType bound) const;
+ bool getDoubleBoundSetup(const SupInfType bound) const;
+
+public: // Modifiors
+ void setDoubleBound(SupInfType boundType, double boundValue);
+ void unsetDoubleBound(SupInfType boundType);
+ void setSingleBound(double boundValue);
+
+public:
+ SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const MultiBound &m);
+ SOT_CORE_EXPORT friend std::istream &operator>>(std::istream &is,
+ MultiBound &m);
};
/* --------------------------------------------------------------------- */
-typedef std::vector< MultiBound > VectorMultiBound;
-SOT_CORE_EXPORT std::ostream& operator<< (std::ostream& os, const VectorMultiBound& v );
-SOT_CORE_EXPORT std::istream& operator>> (std::istream& os, VectorMultiBound& v );
-
-} /* namespace sot */} /* namespace dynamicgraph */
+typedef std::vector<MultiBound> VectorMultiBound;
+SOT_CORE_EXPORT std::ostream &operator<<(std::ostream &os,
+ const VectorMultiBound &v);
+SOT_CORE_EXPORT std::istream &operator>>(std::istream &os, VectorMultiBound &v);
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_MultiBound_H__
diff --git a/include/sot/core/neck-limitation.hh b/include/sot/core/neck-limitation.hh
index 3145c773..0a3b80eb 100644
--- a/include/sot/core/neck-limitation.hh
+++ b/include/sot/core/neck-limitation.hh
@@ -19,78 +19,73 @@
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <sot/core/task-abstract.hh>
/* STD */
-#include <string>
-#include <map>
#include <list>
+#include <map>
+#include <string>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (neck_limitation_EXPORTS)
-# define NeckLimitation_EXPORT __declspec(dllexport)
-# else
-# define NeckLimitation_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(neck_limitation_EXPORTS)
+#define NeckLimitation_EXPORT __declspec(dllexport)
+#else
+#define NeckLimitation_EXPORT __declspec(dllimport)
+#endif
#else
-# define NeckLimitation_EXPORT
+#define NeckLimitation_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class NeckLimitation_EXPORT NeckLimitation
-:public dg::Entity
-{
- public:
+class NeckLimitation_EXPORT NeckLimitation : public dg::Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- unsigned int panRank,tiltRank;
+protected:
+ unsigned int panRank, tiltRank;
static const unsigned int PAN_RANK_DEFAULT;
static const unsigned int TILT_RANK_DEFAULT;
-
/* The limitation is: sgn.Tilt >= Pan.alpha + beta, with alpha the linear
- * coefficient and beta the affine one, and sgn is +1 or -1. */
- double coeffLinearPan,coeffAffinePan;
+ * coefficient and beta the affine one, and sgn is +1 or -1. */
+ double coeffLinearPan, coeffAffinePan;
double signTilt;
static const double COEFF_LINEAR_DEFAULT;
static const double COEFF_AFFINE_DEFAULT;
static const double SIGN_TILT_DEFAULT;
- public: /* --- CONSTRUCTION --- */
-
- NeckLimitation( const std::string& name );
- virtual ~NeckLimitation( void );
+public: /* --- CONSTRUCTION --- */
+ NeckLimitation(const std::string &name);
+ virtual ~NeckLimitation(void);
- public: /* --- SIGNAL --- */
- dg::SignalPtr<dg::Vector,int> jointSIN;
- dg::SignalTimeDependent<dg::Vector,int> jointSOUT;
+public: /* --- SIGNAL --- */
+ dg::SignalPtr<dg::Vector, int> jointSIN;
+ dg::SignalTimeDependent<dg::Vector, int> jointSOUT;
- public: /* --- FUNCTIONS --- */
- dg::Vector& computeJointLimitation( dg::Vector& jointLimited,const int& timeSpec );
+public: /* --- FUNCTIONS --- */
+ dg::Vector &computeJointLimitation(dg::Vector &jointLimited,
+ const int &timeSpec);
- public: /* --- PARAMS --- */
- virtual void display( std::ostream& os ) const;
+public: /* --- PARAMS --- */
+ virtual void display(std::ostream &os) const;
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOT_NeckLimitation_H__
diff --git a/include/sot/core/op-point-modifier.hh b/include/sot/core/op-point-modifier.hh
index 0700b403..4b442acf 100644
--- a/include/sot/core/op-point-modifier.hh
+++ b/include/sot/core/op-point-modifier.hh
@@ -10,8 +10,8 @@
#ifndef __SOT_OP_POINT_MODIFIOR_H__
#define __SOT_OP_POINT_MODIFIOR_H__
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <sot/core/debug.hh>
#include <sot/core/matrix-geometry.hh>
@@ -23,21 +23,22 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (op_point_modifier_EXPORTS)
-# define SOTOPPOINTMODIFIER_EXPORT __declspec(dllexport)
-# else
-# define SOTOPPOINTMODIFIER_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(op_point_modifier_EXPORTS)
+#define SOTOPPOINTMODIFIER_EXPORT __declspec(dllexport)
+#else
+#define SOTOPPOINTMODIFIER_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTOPPOINTMODIFIER_EXPORT
+#define SOTOPPOINTMODIFIER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
///
@@ -46,41 +47,40 @@ namespace dg = dynamicgraph;
/// The position of the local frame in the frame of the joint is represented by
/// transformation.
///
-class SOTOPPOINTMODIFIER_EXPORT OpPointModifier
-: public dg::Entity
-{
- public:
+class SOTOPPOINTMODIFIER_EXPORT OpPointModifier : public dg::Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- public:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- dg::SignalPtr<dg::Matrix,int> jacobianSIN;
- dg::SignalPtr<MatrixHomogeneous,int> positionSIN;
+public:
+ dg::SignalPtr<dg::Matrix, int> jacobianSIN;
+ dg::SignalPtr<MatrixHomogeneous, int> positionSIN;
- dg::SignalTimeDependent<dg::Matrix,int> jacobianSOUT;
- dg::SignalTimeDependent<MatrixHomogeneous,int> positionSOUT;
+ dg::SignalTimeDependent<dg::Matrix, int> jacobianSOUT;
+ dg::SignalTimeDependent<MatrixHomogeneous, int> positionSOUT;
public:
- OpPointModifier( const std::string& name );
- virtual ~OpPointModifier( void ){}
+ OpPointModifier(const std::string &name);
+ virtual ~OpPointModifier(void) {}
- dg::Matrix& jacobianSOUT_function( dg::Matrix& res,const int& time );
- MatrixHomogeneous& positionSOUT_function( MatrixHomogeneous& res,const int& time );
- void setTransformation( const Eigen::Matrix4d& tr );
- void setTransformationBySignalName( std::istringstream& cmdArgs );
- const Eigen::Matrix4d& getTransformation( void );
+ dg::Matrix &jacobianSOUT_function(dg::Matrix &res, const int &time);
+ MatrixHomogeneous &positionSOUT_function(MatrixHomogeneous &res,
+ const int &time);
+ void setTransformation(const Eigen::Matrix4d &tr);
+ 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 the output
- * velocity is expressed in the end-effector frame) of from the world-ref Jacobian (ie
+ /* This bool tunes the effect of the modifier for end-effector Jacobian (ie
+ * the output velocity is expressed in the end-effector frame) of from the
+ * world-ref Jacobian (ie
* the ouput velocity is computed in the world frame). */
bool isEndEffector;
};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // __SOT_OP_POINT_MODIFIOR_H__
diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh
index 3cfca559..89a76b8a 100644
--- a/include/sot/core/parameter-server.hh
+++ b/include/sot/core/parameter-server.hh
@@ -21,101 +21,98 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (__sot_torque_parameter_server_H__)
-# define SOTParameterServer_EXPORT __declspec(dllexport)
-# else
-# define SOTParameterServer_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(__sot_torque_parameter_server_H__)
+#define SOTParameterServer_EXPORT __declspec(dllexport)
#else
-# define SOTParameterServer_EXPORT
+#define SOTParameterServer_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTParameterServer_EXPORT
#endif
-
/* --------------------------------------------------------------------- */
/* --- 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 <map>
-#include "boost/assign.hpp"
-
-
namespace dynamicgraph {
- namespace sot {
+namespace sot {
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- /// Number of time step to transition from one ctrl mode to another
+/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
-
- class SOTParameterServer_EXPORT ParameterServer
- :public::dynamicgraph::Entity
- {
- typedef ParameterServer EntityClassName;
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
- /* --- CONSTRUCTOR ---- */
- ParameterServer( const std::string & name);
-
- /// Initialize
- /// @param dt: control interval
- /// @param urdfFile: path to the URDF model of the robot
- void init(const double & dt,
- const std::string & urdfFile,
- const std::string & robotRef);
-
- /* --- SIGNALS --- */
-
-
- /* --- COMMANDS --- */
-
- /// Commands related to joint name and joint id
- void setNameToId(const std::string& jointName, const double & jointId);
- void setJointLimitsFromId(const double &jointId, const double &lq, const double &uq);
-
- /// Command related to ForceUtil
- void setForceLimitsFromId(const double &jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq);
- void setForceNameToForceId(const std::string& forceName, const double & forceId);
-
- /// Commands related to FootUtil
- void setRightFootSoleXYZ(const dynamicgraph::Vector &);
- void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
- void setFootFrameName(const std::string &, const std::string &);
- void setImuJointName(const std::string &);
- void displayRobotUtil();
- /// Set the mapping between urdf and sot.
- void setJoints(const dynamicgraph::Vector &);
-
- /* --- ENTITY INHERITANCE --- */
- virtual void display( std::ostream& os ) const;
-
- protected:
- RobotUtilShrPtr m_robot_util;
- 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
- int m_iter;
- 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
-
- } // namespace sot
-} // namespace dynamicgraph
-
-
+class SOTParameterServer_EXPORT ParameterServer
+ : public ::dynamicgraph::Entity {
+ typedef ParameterServer EntityClassName;
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+public:
+ /* --- CONSTRUCTOR ---- */
+ ParameterServer(const std::string &name);
+
+ /// Initialize
+ /// @param dt: control interval
+ /// @param urdfFile: path to the URDF model of the robot
+ void init(const double &dt, const std::string &urdfFile,
+ const std::string &robotRef);
+
+ /* --- SIGNALS --- */
+
+ /* --- COMMANDS --- */
+
+ /// Commands related to joint name and joint id
+ void setNameToId(const std::string &jointName, const double &jointId);
+ void setJointLimitsFromId(const double &jointId, const double &lq,
+ const double &uq);
+
+ /// Command related to ForceUtil
+ void setForceLimitsFromId(const double &jointId,
+ const dynamicgraph::Vector &lq,
+ const dynamicgraph::Vector &uq);
+ void setForceNameToForceId(const std::string &forceName,
+ const double &forceId);
+
+ /// Commands related to FootUtil
+ void setRightFootSoleXYZ(const dynamicgraph::Vector &);
+ void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
+ void setFootFrameName(const std::string &, const std::string &);
+ void setImuJointName(const std::string &);
+ void displayRobotUtil();
+ /// Set the mapping between urdf and sot.
+ void setJoints(const dynamicgraph::Vector &);
+
+ /* --- ENTITY INHERITANCE --- */
+ virtual void display(std::ostream &os) const;
+
+protected:
+ RobotUtilShrPtr m_robot_util;
+ 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
+ int m_iter;
+ 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
+
+} // namespace sot
+} // namespace dynamicgraph
#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 c6ecd4eb..eb7bc82c 100644
--- a/include/sot/core/periodic-call-entity.hh
+++ b/include/sot/core/periodic-call-entity.hh
@@ -14,12 +14,11 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/* SOT */
+#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
-#include <sot/core/periodic-call.hh>
#include <sot/core/periodic-call-entity.hh>
-#include <dynamic-graph/all-signals.h>
+#include <sot/core/periodic-call.hh>
/* STD */
#include <list>
#include <map>
@@ -29,14 +28,14 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (periodic_call_entity_EXPORTS)
-# define PeriodicCallEntity_EXPORT __declspec(dllexport)
-# else
-# define PeriodicCallEntity_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(periodic_call_entity_EXPORTS)
+#define PeriodicCallEntity_EXPORT __declspec(dllexport)
+#else
+#define PeriodicCallEntity_EXPORT __declspec(dllimport)
+#endif
#else
-# define PeriodicCallEntity_EXPORT
+#define PeriodicCallEntity_EXPORT
#endif
/* --------------------------------------------------------------------- */
@@ -54,26 +53,27 @@ namespace sot {
If the trigerOnce is called, the stacks are flushed after the execution.
*/
class PeriodicCallEntity_EXPORT PeriodicCallEntity
-: public Entity, protected sot::PeriodicCall
-{
+ : public Entity,
+ protected sot::PeriodicCall {
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- Signal<int,int> triger;
- Signal<int,int> trigerOnce;
+ Signal<int, int> triger;
+ Signal<int, int> trigerOnce;
- int& trigerCall( int& dummy,const int & time );
- int& trigerOnceCall( int& dummy,const int & time );
+ int &trigerCall(int &dummy, const int &time);
+ int &trigerOnceCall(int &dummy, const int &time);
- /* --- FUNCTIONS ------------------------------------------------------------ */
- public:
- PeriodicCallEntity( const std::string& name );
- virtual ~PeriodicCallEntity( void ) {}
+ /* --- FUNCTIONS ------------------------------------------------------------
+ */
+public:
+ PeriodicCallEntity(const std::string &name);
+ virtual ~PeriodicCallEntity(void) {}
- virtual void display( std::ostream& os ) const;
-} ;
+ virtual void display(std::ostream &os) const;
+};
} // namespace sot
} // namespace dynamicgraph
diff --git a/include/sot/core/periodic-call.hh b/include/sot/core/periodic-call.hh
index a6fcbbfc..08a04fd5 100644
--- a/include/sot/core/periodic-call.hh
+++ b/include/sot/core/periodic-call.hh
@@ -14,10 +14,9 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/* SOT */
-#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-base.h>
#include <sot/core/api.hh>
/* STD */
#include <list>
@@ -34,61 +33,58 @@ namespace sot {
/*!
\class PeriodicCall
*/
-class SOT_CORE_EXPORT PeriodicCall
-{
- protected:
- struct SignalToCall
- {
- dynamicgraph::SignalBase<int>* signal;
+class SOT_CORE_EXPORT PeriodicCall {
+protected:
+ struct SignalToCall {
+ dynamicgraph::SignalBase<int> *signal;
unsigned int downsamplingFactor;
- SignalToCall()
- {
+ SignalToCall() {
signal = NULL;
downsamplingFactor = 1;
}
- SignalToCall(dynamicgraph::SignalBase<int>* s, unsigned int df=1)
- {
+ SignalToCall(dynamicgraph::SignalBase<int> *s, unsigned int df = 1) {
signal = s;
downsamplingFactor = df;
}
};
- typedef std::map< std::string,SignalToCall > SignalMapType;
+ typedef std::map<std::string, SignalToCall> SignalMapType;
SignalMapType signalMap;
int innerTime;
- /* --- FUNCTIONS ------------------------------------------------------------ */
- public:
- PeriodicCall( void );
- virtual ~PeriodicCall( void ) {}
+ /* --- FUNCTIONS ------------------------------------------------------------
+ */
+public:
+ PeriodicCall(void);
+ virtual ~PeriodicCall(void) {}
- void addDownsampledSignal( const std::string &name, dynamicgraph::SignalBase<int>& sig, const unsigned int& downsamplingFactor );
- void addDownsampledSignal( const std::string& sigpath, const unsigned int& downsamplingFactor );
+ void addDownsampledSignal(const std::string &name,
+ dynamicgraph::SignalBase<int> &sig,
+ const unsigned int &downsamplingFactor);
+ void addDownsampledSignal(const std::string &sigpath,
+ const unsigned int &downsamplingFactor);
- void addSignal( const std::string &name, dynamicgraph::SignalBase<int>& sig );
- void addSignal( const std::string& args );
- void rmSignal( const std::string &name );
+ void addSignal(const std::string &name, dynamicgraph::SignalBase<int> &sig);
+ void addSignal(const std::string &args);
+ void rmSignal(const std::string &name);
- void runSignals( const int& t );
- void run( const int& t );
+ void runSignals(const int &t);
+ void run(const int &t);
- void clear( void ) { signalMap.clear(); }
+ void clear(void) { signalMap.clear(); }
- void display( std::ostream& os ) const;
- void addSpecificCommands( dynamicgraph::Entity& ent,
- dynamicgraph::Entity::CommandMap_t& commap,
- const std::string & prefix = "" );
+ void display(std::ostream &os) const;
+ void addSpecificCommands(dynamicgraph::Entity &ent,
+ dynamicgraph::Entity::CommandMap_t &commap,
+ const std::string &prefix = "");
};
-
-
} // namespace sot
} // namespace dynamicgraph
-
#endif // #ifndef __SOT_PERIODICCALL_HH__
/*
diff --git a/include/sot/core/pool.hh b/include/sot/core/pool.hh
index 08efc4db..87a92a67 100644
--- a/include/sot/core/pool.hh
+++ b/include/sot/core/pool.hh
@@ -16,26 +16,26 @@
/* --- STD --- */
#include <map>
-#include <string>
#include <sstream>
+#include <string>
/* --- SOT --- */
-#include <sot/core/exception-factory.hh>
-#include <dynamic-graph/signal-base.h>
#include "sot/core/api.hh"
#include <dynamic-graph/pool.h>
+#include <dynamic-graph/signal-base.h>
+#include <sot/core/exception-factory.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
// Preliminary declarations
class FeatureAbstract;
class TaskAbstract;
-
/*! @ingroup factory
\brief This singleton class keep tracks of all features and tasks.
@@ -58,20 +58,19 @@ class TaskAbstract;
It also returns references to signals from their fully-qualified names.
*/
-class SOT_CORE_EXPORT PoolStorage
-{
- public:
+class SOT_CORE_EXPORT PoolStorage {
+public:
/*! \name Define types to simplify the writing
@{
*/
/*! \brief Sorted set of tasks with unique key (name). */
- typedef std::map< std::string,TaskAbstract* > Tasks;
+ typedef std::map<std::string, TaskAbstract *> Tasks;
/*! \brief Sorted set of features with unique key (name). */
- typedef std::map< std::string,FeatureAbstract* > Features;
+ 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.
@{
@@ -84,12 +83,12 @@ class SOT_CORE_EXPORT PoolStorage
Features feature;
/*! @} */
- public:
+public:
/*! \brief Default destructor */
- ~PoolStorage( void );
+ ~PoolStorage(void);
/// \brief Get unique instance of the class
- static PoolStorage* getInstance();
+ static PoolStorage *getInstance();
/// \brief destroy unique instance of the class
static void destroy();
@@ -98,30 +97,32 @@ class SOT_CORE_EXPORT PoolStorage
@{
*/
/*! \brief Registering a feature. */
- void registerFeature( const std::string& entname,FeatureAbstract* ent );
+ void registerFeature(const std::string &entname, FeatureAbstract *ent);
/*! \brief Get a reference to a feature. */
- FeatureAbstract& getFeature( const std::string& name );
+ FeatureAbstract &getFeature(const std::string &name);
/*! @} */
/*! \name Methods related to the handling of the tasks
@{
*/
/*! \brief Registering a task. */
- void registerTask( const std::string& entname,TaskAbstract* ent );
+ void registerTask(const std::string &entname, TaskAbstract *ent);
/*! \brief Get a reference to a task. */
- TaskAbstract& getTask( const std::string& name );
+ TaskAbstract &getTask(const std::string &name);
/*! @} */
- /*! \brief This method write a graph description on the file named FileName. */
+ /*! \brief This method write a graph description on the file named FileName.
+ */
void writeGraph(const std::string &aFileName);
- void writeCompletionList(std::ostream& os);
+ void writeCompletionList(std::ostream &os);
- private:
+private:
PoolStorage();
- static PoolStorage* instance_;
+ static PoolStorage *instance_;
};
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_POOL_HH__ */
diff --git a/include/sot/core/reader.hh b/include/sot/core/reader.hh
index 6df53b8b..ac89cd65 100644
--- a/include/sot/core/reader.hh
+++ b/include/sot/core/reader.hh
@@ -19,32 +19,32 @@
namespace dg = dynamicgraph;
/* STD */
-#include <string>
-#include <vector>
-#include <list>
#include <boost/function.hpp>
#include <fstream>
+#include <list>
+#include <string>
+#include <vector>
/* SOT & DG*/
-#include <dynamic-graph/signal-base.h>
-#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/exception-traces.h>
+#include <dynamic-graph/signal-base.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/flags.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (reader_EXPORTS)
-# define SOTREADER_EXPORT __declspec(dllexport)
-# else
-# define SOTREADER_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(reader_EXPORTS)
+#define SOTREADER_EXPORT __declspec(dllexport)
#else
-# define SOTREADER_EXPORT
+#define SOTREADER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTREADER_EXPORT
#endif
/* --------------------------------------------------------------------- */
@@ -56,45 +56,38 @@ using dynamicgraph::SignalPtr;
using dynamicgraph::SignalTimeDependent;
using dynamicgraph::sot::Flags;
-class SOTREADER_EXPORT sotReader
-: public Entity
-{
+class SOTREADER_EXPORT sotReader : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
- public:
-
- SignalPtr< Flags,int > selectionSIN;
- SignalTimeDependent<dg::Vector,int> vectorSOUT;
- SignalTimeDependent<dg::Matrix,int> matrixSOUT;
- public:
- sotReader( const std::string n );
- virtual ~sotReader( void ){}
+public:
+ SignalPtr<Flags, int> selectionSIN;
+ SignalTimeDependent<dg::Vector, int> vectorSOUT;
+ SignalTimeDependent<dg::Matrix, int> matrixSOUT;
- void load( const std::string& filename );
- void clear( void );
- void rewind( void );
+public:
+ sotReader(const std::string n);
+ virtual ~sotReader(void) {}
- protected:
+ void load(const std::string &filename);
+ void clear(void);
+ void rewind(void);
- typedef std::list< std::vector<double> > DataType;
+protected:
+ typedef std::list<std::vector<double>> DataType;
DataType dataSet;
DataType::const_iterator currentData;
bool iteratorSet;
- int rows,cols;
+ int rows, cols;
- dg::Vector& getNextData( dg::Vector& res, const unsigned int time );
- dg::Matrix& getNextMatrix( dg::Matrix& res, const unsigned int time );
- void resize(const int & nbRow, const int & nbCol);
+ dg::Vector &getNextData(dg::Vector &res, const unsigned int time);
+ dg::Matrix &getNextMatrix(dg::Matrix &res, const unsigned int time);
+ void resize(const int &nbRow, const int &nbCol);
- public:
+public:
/* --- PARAMS --- */
- void display( std::ostream& os ) const;
+ void display(std::ostream &os) const;
virtual void initCommands();
};
-
-
-
-
#endif /* #ifndef __SOT_TRACER_H__ */
diff --git a/include/sot/core/robot-simu.hh b/include/sot/core/robot-simu.hh
index 2c3da3e7..e37c8382 100644
--- a/include/sot/core/robot-simu.hh
+++ b/include/sot/core/robot-simu.hh
@@ -19,44 +19,39 @@
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
-#include "sot/core/device.hh"
#include "sot/core/api.hh"
+#include "sot/core/device.hh"
+#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (robot_simu_EXPORTS)
-# define SOT_ROBOT_SIMU_EXPORT __declspec(dllexport)
-# else
-# define SOT_ROBOT_SIMU_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(robot_simu_EXPORTS)
+#define SOT_ROBOT_SIMU_EXPORT __declspec(dllexport)
#else
-# define SOT_ROBOT_SIMU_EXPORT
+#define SOT_ROBOT_SIMU_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOT_ROBOT_SIMU_EXPORT
#endif
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class SOT_ROBOT_SIMU_EXPORT RobotSimu
- :public Device
- {
- 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 {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+class SOT_ROBOT_SIMU_EXPORT RobotSimu : public Device {
+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
#endif /* #ifndef DYNAMICGRAPH_SOT_ROBOT_SIMU_HH */
diff --git a/include/sot/core/robot-utils.hh b/include/sot/core/robot-utils.hh
index d259737f..a8b23938 100644
--- a/include/sot/core/robot-utils.hh
+++ b/include/sot/core/robot-utils.hh
@@ -5,283 +5,228 @@
*
*/
-
#ifndef __sot_torque_control_common_H__
#define __sot_torque_control_common_H__
-
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+#include "boost/assign.hpp"
#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/logger.h>
#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
#include <map>
-#include "boost/assign.hpp"
-#include <dynamic-graph/logger.h>
+#include <sot/core/matrix-geometry.hh>
namespace dg = ::dynamicgraph;
using namespace dg;
-
namespace dynamicgraph {
- namespace sot {
+namespace sot {
+
+struct SOT_CORE_EXPORT JointLimits {
+ double upper;
+ double lower;
+
+ JointLimits() : upper(0.0), lower(0.0) {}
+
+ JointLimits(double l, double u) : upper(u), lower(l) {}
+};
+
+typedef Eigen::VectorXd::Index Index;
+
+struct SOT_CORE_EXPORT ForceLimits {
+ Eigen::VectorXd upper;
+ Eigen::VectorXd lower;
+
+ ForceLimits() : upper(Vector6d::Zero()), lower(Vector6d::Zero()) {}
+
+ ForceLimits(const Eigen::VectorXd &l, const Eigen::VectorXd &u)
+ : upper(u), lower(l) {}
+
+ void display(std::ostream &os) const;
+};
+
+struct SOT_CORE_EXPORT ForceUtil {
+ std::map<Index, ForceLimits> m_force_id_to_limits;
+ std::map<std::string, Index> m_name_to_force_id;
+ std::map<Index, std::string> m_force_id_to_name;
+
+ Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand, m_Force_Id_Left_Foot,
+ m_Force_Id_Right_Foot;
+
+ void set_name_to_force_id(const std::string &name, const Index &force_id);
+
+ void set_force_id_to_limits(const Index &force_id, const dg::Vector &lf,
+ const dg::Vector &uf);
+
+ void create_force_id_to_name_map();
+
+ Index get_id_from_name(const std::string &name);
+
+ const std::string &get_name_from_id(Index idx);
+ std::string cp_get_name_from_id(Index idx);
+
+ const ForceLimits &get_limits_from_id(Index force_id);
+ ForceLimits cp_get_limits_from_id(Index force_id);
+
+ Index get_force_id_left_hand() { return m_Force_Id_Left_Hand; }
+
+ void set_force_id_left_hand(Index anId) { m_Force_Id_Left_Hand = anId; }
+
+ Index get_force_id_right_hand() { return m_Force_Id_Right_Hand; }
- struct SOT_CORE_EXPORT JointLimits
- {
- double upper;
- double lower;
+ void set_force_id_right_hand(Index anId) { m_Force_Id_Right_Hand = anId; }
- JointLimits():
- upper(0.0),
- lower(0.0)
- {}
+ Index get_force_id_left_foot() { return m_Force_Id_Left_Foot; }
- JointLimits(double l, double u):
- upper(u),
- lower(l)
- {}
- };
+ void set_force_id_left_foot(Index anId) { m_Force_Id_Left_Foot = anId; }
- typedef Eigen::VectorXd::Index Index;
+ Index get_force_id_right_foot() { return m_Force_Id_Right_Foot; }
- struct SOT_CORE_EXPORT ForceLimits
- {
- Eigen::VectorXd upper;
- Eigen::VectorXd lower;
+ void set_force_id_right_foot(Index anId) { m_Force_Id_Right_Foot = anId; }
- ForceLimits():
- upper(Vector6d::Zero()),
- lower(Vector6d::Zero())
- {}
+ void display(std::ostream &out) const;
- ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
- upper(u),
- lower(l)
- {}
+}; // struct ForceUtil
- void display(std::ostream &os) const;
- };
+struct SOT_CORE_EXPORT FootUtil {
+ /// Position of the foot soles w.r.t. the frame of the foot
+ dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
+ /// Position of the force/torque sensors w.r.t. the frame of the hosting link
+ dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
+ std::string m_Left_Foot_Frame_Name;
+ std::string m_Right_Foot_Frame_Name;
+ void display(std::ostream &os) const;
+};
+struct SOT_CORE_EXPORT HandUtil {
+ std::string m_Left_Hand_Frame_Name;
+ std::string m_Right_Hand_Frame_Name;
+ void display(std::ostream &os) const;
+};
- struct SOT_CORE_EXPORT ForceUtil
- {
- std::map<Index,ForceLimits> m_force_id_to_limits;
- std::map<std::string,Index> m_name_to_force_id;
- std::map<Index,std::string> m_force_id_to_name;
+struct SOT_CORE_EXPORT RobotUtil {
+public:
+ RobotUtil();
- Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand,
- m_Force_Id_Left_Foot, m_Force_Id_Right_Foot;
+ /// Forces data
+ ForceUtil m_force_util;
- void set_name_to_force_id(const std::string & name,
- const Index &force_id);
+ /// Foot information
+ FootUtil m_foot_util;
+ /// Hand information
+ HandUtil m_hand_util;
- void set_force_id_to_limits(const Index &force_id,
- const dg::Vector &lf,
- const dg::Vector &uf);
+ /// Map from the urdf index to the SoT index.
+ std::vector<Index> m_urdf_to_sot;
- void create_force_id_to_name_map();
+ /// Nb of Dofs for the robot.
+ std::size_t m_nbJoints;
+ /// Map from the name to the id.
+ std::map<std::string, Index> m_name_to_id;
- Index get_id_from_name(const std::string &name);
+ /// The map between id and name
+ std::map<Index, std::string> m_id_to_name;
- const std::string & get_name_from_id(Index idx);
- std::string cp_get_name_from_id(Index idx);
+ /// The joint limits map.
+ std::map<Index, JointLimits> m_limits_map;
- const ForceLimits & get_limits_from_id(Index force_id);
- ForceLimits cp_get_limits_from_id(Index force_id);
+ /// The name of the joint IMU is attached to
+ std::string m_imu_joint_name;
- Index get_force_id_left_hand()
- {
- return m_Force_Id_Left_Hand;
- }
+ /// This method creates the map between id and name.
+ /// It is called each time a new link between id and name is inserted
+ /// (i.e. when set_name_to_id is called).
+ void create_id_to_name_map();
- void set_force_id_left_hand(Index anId)
- {
- m_Force_Id_Left_Hand = anId;
- }
+ /// URDF file path
+ std::string m_urdf_filename;
- Index get_force_id_right_hand()
- {
- return m_Force_Id_Right_Hand;
- }
+ dynamicgraph::Vector m_dgv_urdf_to_sot;
- void set_force_id_right_hand( Index anId)
- {
- m_Force_Id_Right_Hand = anId;
- }
+ /** Given a joint name it finds the associated joint id.
+ * If the specified joint name is not found it returns -1;
+ * @param name Name of the joint to find.
+ * @return The id of the specified joint, -1 if not found. */
+ const Index &get_id_from_name(const std::string &name);
- Index get_force_id_left_foot()
- {
- return m_Force_Id_Left_Foot;
- }
+ /** Given a joint id it finds the associated joint name.
+ * If the specified joint is not found it returns "Joint name not found";
+ * @param id Id of the joint to find.
+ * @return The name of the specified joint, "Joint name not found" if not
+ * found. */
- void set_force_id_left_foot(Index anId)
- {
- m_Force_Id_Left_Foot = anId;
- }
+ /// Get the joint name from its index
+ const std::string &get_name_from_id(Index id);
- Index get_force_id_right_foot()
- {
- return m_Force_Id_Right_Foot;
- }
+ /// Set relation between the name and the SoT id
+ void set_name_to_id(const std::string &jointName, const Index &jointId);
- void set_force_id_right_foot( Index anId)
- {
- m_Force_Id_Right_Foot = anId;
- }
+ /// Set the map between urdf index and sot index
+ void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
+ void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
- void display(std::ostream & out) const;
+ /// Set the limits (lq,uq) for joint idx
+ void set_joint_limits_for_id(const Index &idx, const double &lq,
+ const double &uq);
- }; // struct ForceUtil
+ bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
- struct SOT_CORE_EXPORT FootUtil
- {
- /// Position of the foot soles w.r.t. the frame of the foot
- dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
- /// Position of the force/torque sensors w.r.t. the frame of the hosting link
- dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
- std::string m_Left_Foot_Frame_Name;
- std::string m_Right_Foot_Frame_Name;
- void display(std::ostream & os) const;
- };
+ bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
- struct SOT_CORE_EXPORT HandUtil
- {
- std::string m_Left_Hand_Frame_Name;
- std::string m_Right_Hand_Frame_Name;
- void display(std::ostream & os) const;
- };
+ bool velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
+ RefVector v_sot);
- struct SOT_CORE_EXPORT RobotUtil
- {
- public:
+ bool velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
+ RefVector v_urdf);
- RobotUtil();
+ bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
+ bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
- /// Forces data
- ForceUtil m_force_util;
+ bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
+ bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
- /// Foot information
- FootUtil m_foot_util;
+ /** Given a joint id it finds the associated joint limits.
+ * If the specified joint is not found it returns JointLimits(0,0).
+ * @param id Id of the joint to find.
+ * @return The limits of the specified joint, JointLimits(0,0) if not found.
+ */
+ const JointLimits &get_joint_limits_from_id(Index id);
+ JointLimits cp_get_joint_limits_from_id(Index id);
- /// Hand information
- HandUtil m_hand_util;
-
- /// Map from the urdf index to the SoT index.
- std::vector<Index> m_urdf_to_sot;
+ /** \name Logger related methods */
+ /** \{*/
+ /// \brief Send messages \c msg with level \c t. Add string \c file and \c
+ /// line to message.
+ void sendMsg(const std::string &msg, MsgType t = MSG_TYPE_INFO,
+ const char *file = "", int line = 0);
- /// Nb of Dofs for the robot.
- std::size_t m_nbJoints;
+ /// \brief Specify the verbosity level of the logger.
+ void setLoggerVerbosityLevel(LoggerVerbosity lv) { logger_.setVerbosity(lv); }
- /// Map from the name to the id.
- std::map<std::string,Index> m_name_to_id;
+ /// \brief Get the logger's verbosity level.
+ LoggerVerbosity getLoggerVerbosityLevel() { return logger_.getVerbosity(); };
- /// The map between id and name
- std::map<Index,std::string> m_id_to_name;
+ void display(std::ostream &os) const;
- /// The joint limits map.
- std::map<Index,JointLimits> m_limits_map;
-
- /// The name of the joint IMU is attached to
- std::string m_imu_joint_name;
-
- /// This method creates the map between id and name.
- /// It is called each time a new link between id and name is inserted
- /// (i.e. when set_name_to_id is called).
- void create_id_to_name_map();
-
- /// URDF file path
- std::string m_urdf_filename;
-
- dynamicgraph::Vector m_dgv_urdf_to_sot;
-
- /** Given a joint name it finds the associated joint id.
- * If the specified joint name is not found it returns -1;
- * @param name Name of the joint to find.
- * @return The id of the specified joint, -1 if not found. */
- const Index & get_id_from_name(const std::string &name);
-
- /** Given a joint id it finds the associated joint name.
- * If the specified joint is not found it returns "Joint name not found";
- * @param id Id of the joint to find.
- * @return The name of the specified joint, "Joint name not found" if not found. */
-
- /// Get the joint name from its index
- const std::string & get_name_from_id(Index id);
-
- /// Set relation between the name and the SoT id
- void set_name_to_id(const std::string &jointName,
- const Index & jointId);
-
- /// Set the map between urdf index and sot index
- void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
- void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
-
- /// Set the limits (lq,uq) for joint idx
- void set_joint_limits_for_id(const Index &idx,
- const double &lq,
- const double &uq);
-
- bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
-
- bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
-
- bool velocity_urdf_to_sot(ConstRefVector q_urdf,
- ConstRefVector v_urdf, RefVector v_sot);
-
- bool velocity_sot_to_urdf(ConstRefVector q_urdf,
- ConstRefVector v_sot, RefVector v_urdf);
-
- bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
- bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
-
- bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
- bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
-
-
- /** Given a joint id it finds the associated joint limits.
- * If the specified joint is not found it returns JointLimits(0,0).
- * @param id Id of the joint to find.
- * @return The limits of the specified joint, JointLimits(0,0) if not found. */
- const JointLimits & get_joint_limits_from_id(Index id);
- JointLimits cp_get_joint_limits_from_id(Index id);
-
- /** \name Logger related methods */
- /** \{*/
- /// \brief Send messages \c msg with level \c t. Add string \c file and \c line to message.
- void sendMsg(const std::string &msg,
- MsgType t=MSG_TYPE_INFO,
- const char *file="",
- int line=0);
+protected:
+ Logger logger_;
+}; // struct RobotUtil
- /// \brief Specify the verbosity level of the logger.
- void setLoggerVerbosityLevel(LoggerVerbosity lv)
- {logger_.setVerbosity(lv);}
+/// Accessors - This should be changed to RobotUtilPtrShared
+typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
- /// \brief Get the logger's verbosity level.
- LoggerVerbosity getLoggerVerbosityLevel()
- { return logger_.getVerbosity(); };
-
- void display(std::ostream &os) const;
- protected:
- Logger logger_;
- }; // struct RobotUtil
+RobotUtilShrPtr RefVoidRobotUtil();
+RobotUtilShrPtr getRobotUtil(std::string &robotName);
+bool isNameInRobotUtil(std::string &robotName);
+RobotUtilShrPtr createRobotUtil(std::string &robotName);
- /// Accessors - This should be changed to RobotUtilPtrShared
- typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
+bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot);
- RobotUtilShrPtr RefVoidRobotUtil();
- RobotUtilShrPtr getRobotUtil(std::string &robotName);
- bool isNameInRobotUtil(std::string &robotName);
- RobotUtilShrPtr createRobotUtil(std::string &robotName);
-
- 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_
diff --git a/include/sot/core/seq-play.hh b/include/sot/core/seq-play.hh
index 1487df22..dd791ac7 100644
--- a/include/sot/core/seq-play.hh
+++ b/include/sot/core/seq-play.hh
@@ -18,8 +18,8 @@
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <list>
@@ -27,61 +27,61 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (seq_play_EXPORTS)
-# define SOTSEQPLAY_EXPORT __declspec(dllexport)
-# else
-# define SOTSEQPLAY_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(seq_play_EXPORTS)
+#define SOTSEQPLAY_EXPORT __declspec(dllexport)
+#else
+#define SOTSEQPLAY_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTSEQPLAY_EXPORT
+#define SOTSEQPLAY_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
-class SOTSEQPLAY_EXPORT SeqPlay
-:public dynamicgraph::Entity
-{
- public:
+class SOTSEQPLAY_EXPORT SeqPlay : public dynamicgraph::Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- typedef std::list<dg::Vector> StateList;
+protected:
+ typedef std::list<dg::Vector> StateList;
StateList stateList;
- StateList::iterator currPos; unsigned int currRank;
+ StateList::iterator currPos;
+ unsigned int currRank;
bool init;
int time;
- public:
-
+public:
/* --- CONSTRUCTION --- */
- SeqPlay( const std::string& name );
- virtual ~SeqPlay( void ) { }
-
- void loadFile( const std::string& name );
-
- dg::Vector& getNextPosition( dg::Vector& pos, const int& time );
-
- public: /* --- DISPLAY --- */
- virtual void display( std::ostream& os ) const;
- SOTSEQPLAY_EXPORT friend std::ostream& operator<< ( std::ostream& os,const SeqPlay& r )
- { r.display(os); return os;}
-
- public: /* --- SIGNALS --- */
-
- //dynamicgraph::SignalPtr<dg::Vector,int> positionSIN;
- //dynamicgraph::SignalTimeDependant<dg::Vector,int> velocitySOUT;
- dynamicgraph::SignalTimeDependent<int,int> refresherSINTERN;
- dynamicgraph::SignalTimeDependent<dg::Vector,int> positionSOUT;
+ SeqPlay(const std::string &name);
+ virtual ~SeqPlay(void) {}
+
+ void loadFile(const std::string &name);
+
+ dg::Vector &getNextPosition(dg::Vector &pos, const int &time);
+
+public: /* --- DISPLAY --- */
+ virtual void display(std::ostream &os) const;
+ SOTSEQPLAY_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const SeqPlay &r) {
+ r.display(os);
+ return os;
+ }
+
+public: /* --- SIGNALS --- */
+ // dynamicgraph::SignalPtr<dg::Vector,int> positionSIN;
+ // dynamicgraph::SignalTimeDependant<dg::Vector,int> velocitySOUT;
+ dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
+ dynamicgraph::SignalTimeDependent<dg::Vector, int> positionSOUT;
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SEQPLAY_HH */
diff --git a/include/sot/core/seqplay.hh b/include/sot/core/seqplay.hh
index ab1ac32e..dd791ac7 100644
--- a/include/sot/core/seqplay.hh
+++ b/include/sot/core/seqplay.hh
@@ -18,8 +18,8 @@
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <list>
@@ -27,62 +27,61 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (seq_play_EXPORTS)
-# define SOTSEQPLAY_EXPORT __declspec(dllexport)
-# else
-# define SOTSEQPLAY_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(seq_play_EXPORTS)
+#define SOTSEQPLAY_EXPORT __declspec(dllexport)
+#else
+#define SOTSEQPLAY_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTSEQPLAY_EXPORT
+#define SOTSEQPLAY_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
-class SOTSEQPLAY_EXPORT SeqPlay
-:public dynamicgraph::Entity
-{
- public:
+class SOTSEQPLAY_EXPORT SeqPlay : public dynamicgraph::Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- protected:
-
- typedef std::list<dg::Vector> StateList;
+protected:
+ typedef std::list<dg::Vector> StateList;
StateList stateList;
- StateList::iterator currPos; unsigned int currRank;
+ StateList::iterator currPos;
+ unsigned int currRank;
bool init;
int time;
- public:
-
+public:
/* --- CONSTRUCTION --- */
- SeqPlay( const std::string& name );
- virtual ~SeqPlay( void ) { }
-
- void loadFile( const std::string& name );
-
- dg::Vector& getNextPosition( dg::Vector& pos, const int& time );
-
- public: /* --- DISPLAY --- */
- virtual void display( std::ostream& os ) const;
- SOTSEQPLAY_EXPORT friend std::ostream& operator<< ( std::ostream& os,const SeqPlay& r )
- { r.display(os); return os;}
-
- public: /* --- SIGNALS --- */
-
- //dynamicgraph::SignalPtr<dg::Vector,int> positionSIN;
- //dynamicgraph::SignalTimeDependant<dg::Vector,int> velocitySOUT;
- dynamicgraph::SignalTimeDependent<int,int> refresherSINTERN;
- dynamicgraph::SignalTimeDependent<dg::Vector,int> positionSOUT;
-
+ SeqPlay(const std::string &name);
+ virtual ~SeqPlay(void) {}
+
+ void loadFile(const std::string &name);
+
+ dg::Vector &getNextPosition(dg::Vector &pos, const int &time);
+
+public: /* --- DISPLAY --- */
+ virtual void display(std::ostream &os) const;
+ SOTSEQPLAY_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const SeqPlay &r) {
+ r.display(os);
+ return os;
+ }
+
+public: /* --- SIGNALS --- */
+ // dynamicgraph::SignalPtr<dg::Vector,int> positionSIN;
+ // dynamicgraph::SignalTimeDependant<dg::Vector,int> velocitySOUT;
+ dynamicgraph::SignalTimeDependent<int, int> refresherSINTERN;
+ dynamicgraph::SignalTimeDependent<dg::Vector, int> positionSOUT;
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SEQPLAY_HH */
diff --git a/include/sot/core/sequencer.hh b/include/sot/core/sequencer.hh
index 02169399..0c0584b8 100644
--- a/include/sot/core/sequencer.hh
+++ b/include/sot/core/sequencer.hh
@@ -19,102 +19,93 @@
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <sot/core/task-abstract.hh>
/* STD */
-#include <string>
-#include <map>
#include <list>
+#include <map>
+#include <string>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (sequencer_EXPORTS)
-# define SOTSEQUENCER_EXPORT __declspec(dllexport)
-# else
-# define SOTSEQUENCER_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(sequencer_EXPORTS)
+#define SOTSEQUENCER_EXPORT __declspec(dllexport)
#else
-# define SOTSEQUENCER_EXPORT
+#define SOTSEQUENCER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTSEQUENCER_EXPORT
#endif
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class Sot;
-
- class SOTSEQUENCER_EXPORT Sequencer
- :public dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- class sotEventAbstract
- {
- public:
- enum sotEventType
- {
- EVENT_ADD
- ,EVENT_RM
- ,EVENT_CMD
- };
- protected:
- std::string name;
- void setName( const std::string& name_ ) { name = name_; }
- int eventType;
- public:
- sotEventAbstract( const std::string & name ) : name(name) {};
- virtual ~sotEventAbstract( void ) {}
- virtual const std::string& getName() const { return name; }
- int getEventType( ) const { return eventType; }
- virtual void operator() ( Sot* sotPtr ) = 0;
- virtual void display( std::ostream& os ) const { os << name; }
- };
-
- protected:
- Sot* sotPtr;
- typedef std::list< sotEventAbstract* > TaskList;
- typedef std::map< unsigned int,TaskList > TaskMap;
-
- TaskMap taskMap;
- /* All the events are counting wrt to this t0. If t0 is -1, it
- * is set to the first time of trig. */
- int timeInit;
- bool playMode;
- std::ostream* outputStreamPtr;
- bool noOutput; /*! if true, display nothing standard output on except errors*/
-
- public: /* --- CONSTRUCTION --- */
-
- Sequencer( const std::string& name );
- virtual ~Sequencer( void );
-
- 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 --- */
- dynamicgraph::SignalTimeDependent<int,int> triggerSOUT;
-
- public: /* --- FUNCTIONS --- */
- int& trigger( int& dummy,const int& time );
-
- public: /* --- PARAMS --- */
- virtual void display( std::ostream& os ) const;
- };
- } // namespace sot
-} // namespace dynamicgraph
+namespace sot {
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+class Sot;
+
+class SOTSEQUENCER_EXPORT Sequencer : public dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+public:
+ class sotEventAbstract {
+ public:
+ enum sotEventType { EVENT_ADD, EVENT_RM, EVENT_CMD };
+
+ protected:
+ std::string name;
+ void setName(const std::string &name_) { name = name_; }
+ int eventType;
+
+ public:
+ sotEventAbstract(const std::string &name) : name(name){};
+ virtual ~sotEventAbstract(void) {}
+ virtual const std::string &getName() const { return name; }
+ int getEventType() const { return eventType; }
+ virtual void operator()(Sot *sotPtr) = 0;
+ virtual void display(std::ostream &os) const { os << name; }
+ };
+
+protected:
+ Sot *sotPtr;
+ typedef std::list<sotEventAbstract *> TaskList;
+ typedef std::map<unsigned int, TaskList> TaskMap;
+
+ TaskMap taskMap;
+ /* All the events are counting wrt to this t0. If t0 is -1, it
+ * is set to the first time of trig. */
+ int timeInit;
+ bool playMode;
+ std::ostream *outputStreamPtr;
+ bool noOutput; /*! if true, display nothing standard output on except errors*/
+
+public: /* --- CONSTRUCTION --- */
+ Sequencer(const std::string &name);
+ virtual ~Sequencer(void);
+
+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 --- */
+ dynamicgraph::SignalTimeDependent<int, int> triggerSOUT;
+
+public: /* --- FUNCTIONS --- */
+ int &trigger(int &dummy, const int &time);
+
+public: /* --- PARAMS --- */
+ virtual void display(std::ostream &os) const;
+};
+} // namespace sot
+} // namespace dynamicgraph
#endif // #ifndef __SOT_SOTSEQUENCER_H__
diff --git a/include/sot/core/smooth-reach.hh b/include/sot/core/smooth-reach.hh
index a7b1a166..1240e5b9 100644
--- a/include/sot/core/smooth-reach.hh
+++ b/include/sot/core/smooth-reach.hh
@@ -19,25 +19,25 @@
namespace dg = dynamicgraph;
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (com_freezer_EXPORTS)
-# define SOTSMOOTHREACH_EXPORT __declspec(dllexport)
-# else
-# define SOTSMOOTHREACH_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(com_freezer_EXPORTS)
+#define SOTSMOOTHREACH_EXPORT __declspec(dllexport)
#else
-# define SOTSMOOTHREACH_EXPORT
+#define SOTSMOOTHREACH_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTSMOOTHREACH_EXPORT
#endif
-
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
@@ -45,49 +45,45 @@ namespace dg = dynamicgraph;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-class SOTSMOOTHREACH_EXPORT SmoothReach
-: public dg::Entity
-{
- public:
- static const std::string CLASS_NAME;
- virtual const std::string & getClassName() const { return CLASS_NAME; }
+class SOTSMOOTHREACH_EXPORT SmoothReach : public dg::Entity {
+public:
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName() const { return CLASS_NAME; }
- private:
- dg::Vector start,goal;
- int startTime, lengthTime;
- bool isStarted, isParam;
- int smoothMode; double smoothParam;
+private:
+ dg::Vector start, goal;
+ int startTime, lengthTime;
+ bool isStarted, isParam;
+ int smoothMode;
+ double smoothParam;
- double smoothFunction( double x );
+ double smoothFunction(double x);
- public: /* --- CONSTRUCTION --- */
- SmoothReach(const std::string & name);
- virtual ~SmoothReach(void) {};
+public: /* --- CONSTRUCTION --- */
+ SmoothReach(const std::string &name);
+ virtual ~SmoothReach(void){};
- public: /* --- SIGNAL --- */
- dg::SignalPtr<dg::Vector, int> startSIN;
- dg::SignalTimeDependent<dg::Vector, int> goalSOUT;
- //dg::SignalTimeDependent<double, int> percentSOUT;
+public: /* --- SIGNAL --- */
+ dg::SignalPtr<dg::Vector, int> startSIN;
+ dg::SignalTimeDependent<dg::Vector, int> goalSOUT;
+ // dg::SignalTimeDependent<double, int> percentSOUT;
- public: /* --- FUNCTION --- */
- dg::Vector& goalSOUT_function(dg::Vector & goal, const int& time);
+public: /* --- FUNCTION --- */
+ dg::Vector &goalSOUT_function(dg::Vector &goal, const int &time);
- void set( const dg::Vector & goal, const int & length );
- const dg::Vector & getGoal( void );
- const int & getLength( void );
- const int & getStart( void );
+ void set(const dg::Vector &goal, const int &length);
+ const dg::Vector &getGoal(void);
+ const int &getLength(void);
+ const int &getStart(void);
- void setSmoothing( const int & mode, const double & param );
+ void setSmoothing(const int &mode, const double ¶m);
- public: /* --- PARAMS --- */
- virtual void display(std::ostream & os) const;
- void initCommands( void );
+public: /* --- PARAMS --- */
+ virtual void display(std::ostream &os) const;
+ void initCommands(void);
};
-
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SMOOTHREACH_H_H */
diff --git a/include/sot/core/sot.hh b/include/sot/core/sot.hh
index 4a3cbe4b..7394365a 100644
--- a/include/sot/core/sot.hh
+++ b/include/sot/core/sot.hh
@@ -19,28 +19,28 @@
namespace dg = dynamicgraph;
/* Classes standards. */
-#include <list> /* Classe std::list */
+#include <list> /* Classe std::list */
/* SOT */
-#include <sot/core/task-abstract.hh>
-#include <sot/core/flags.hh>
#include <dynamic-graph/entity.h>
#include <sot/core/constraint.hh>
+#include <sot/core/flags.hh>
+#include <sot/core/task-abstract.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#ifndef SOTSOT_CORE_EXPORT
-# if defined (WIN32)
-# if defined (sot_EXPORTS)
-# define SOTSOT_CORE_EXPORT __declspec(dllexport)
-# else
-# define SOTSOT_CORE_EXPORT __declspec(dllimport)
-# endif
-# else
-# define SOTSOT_CORE_EXPORT
-# endif
+#if defined(WIN32)
+#if defined(sot_EXPORTS)
+#define SOTSOT_CORE_EXPORT __declspec(dllexport)
+#else
+#define SOTSOT_CORE_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTSOT_CORE_EXPORT
+#endif
#endif
/* --------------------------------------------------------------------- */
@@ -48,205 +48,196 @@ namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
-
- /*! @ingroup stackoftasks
- \brief This class implements the Stack of Task.
- It allows to deal with the priority of the controllers
- through the shell. The controllers can be either constraints
- either tasks.
-
-
- */
- class SOTSOT_CORE_EXPORT Sot
- :public Entity
- {
- public:
- /*! \brief Specify the name of the class entity. */
- static const std::string CLASS_NAME;
- 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:
-
- /*! \brief This field is a list of controllers
- managed by the stack of tasks. */
- StackType stack;
-
- /*! \brief Defines a type for a list of constraints */
- typedef std::list<Constraint*> ConstraintListType;
- /*! \brief This field is a list of constraints
- managed by the stack of tasks. */
- ConstraintListType constraintList;
-
- /*! \brief Defines an interval in the state vector of the robot
- which is the free flyer. */
- unsigned int ffJointIdFirst,ffJointIdLast;
- /*! \brief Defines a default joint. */
- static const unsigned int FF_JOINT_ID_DEFAULT = 0;
-
- /* double directionalThreshold; */
- /* bool useContiInverse; */
-
- /*! \brief Store the number of joints to be used in the
- command computed by the stack of tasks. */
- unsigned int nbJoints;
-
- /*! \brief Store a pointer to compute the gradient */
- TaskAbstract* taskGradient;
-
- //Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Proj;
- /*! Force the recomputation at each step. */
- bool recomputeEachTime;
-
- public:
-
- /*! \brief Threshold to compute the dumped pseudo inverse. */
- static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
-
- /* static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
- /* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
-
- /*! \brief Number of joints by default. */
- static dg::Matrix & computeJacobianConstrained( const dg::Matrix& Jac,
- const dg::Matrix& K,
- dg::Matrix& JK);
- static dg::Matrix & computeJacobianConstrained( const TaskAbstract& task,
- const dg::Matrix& K );
- static void
- taskVectorToMlVector(const VectorMultiBound& taskVector, Vector& err);
-
- public:
-
- /*! \brief Default constructor */
- Sot( const std::string& name );
- ~Sot( void ) { /* TODO!! */ }
-
- /*! \name Methods to handle the stack.
- @{
- */
- virtual const StackType& tasks () const { return stack; }
-
- /*! \brief Push the task in the stack.
- It has a lowest priority than the previous ones.
- If this is the first task, then it has the highest
- priority. */
- virtual void push( TaskAbstract& task );
- /*! \brief Pop the task from the stack.
- This method removes the task with the smallest
- priority in the task. The other are projected
- in the null-space of their predecessors. */
- virtual TaskAbstract& pop( void );
-
- /*! \brief This method allows to know if a task exists or not */
- virtual bool exist( const TaskAbstract& task );
-
- /*! \brief Remove a task regardless to its position in the stack.
- It removes also the signals connected to the output signal of this
- stack.*/
- virtual void remove( const TaskAbstract& task );
-
- /*! \brief This method removes the output signals depending on
- this task. */
- virtual void removeDependency( const TaskAbstract& key );
-
- /*! \brief This method makes the task to swap with the task having the
- immediate superior priority. */
- virtual void up( const TaskAbstract& task );
-
- /*! \brief This method makes the task to swap with the task having the
- immediate inferior priority. */
- virtual void down( const TaskAbstract& task );
-
- /*! \brief Remove all the tasks from the stack. */
- virtual void clear( void );
- /*! @} */
-
-
- /*! \name Methods to handle the constraints.
- @{
- */
- /*! \brief Add a constraint to the stack with the current level
- of priority. */
- virtual void addConstraint( Constraint& constraint );
- /*! \brief Remove a constraint from the stack. */
- virtual void removeConstraint( const Constraint& constraint );
- /*! \brief Remove all the constraints from the stack. */
- virtual void clearConstraint( void );
-
- /*! @} */
-
- /*! \brief This method defines the part of the state vector
- which correspond to the free flyer of the robot. */
- virtual void
- defineFreeFloatingJoints(const unsigned int& jointIdFirst,
- const unsigned int& jointIdLast = -1);
- virtual void defineNbDof( const unsigned int& nbDof );
- virtual const unsigned int& getNbDof() const { return nbJoints; }
-
- /*! @} */
- public: /* --- CONTROL --- */
-
- /*! \name Methods to compute the control law following the
- recursive definition of the stack of tasks.
- @{
- */
-
- /*! \brief Compute the control law. */
- virtual dg::Vector& computeControlLaw(dg::Vector& control,
- const int& time);
-
- /*! \brief Compute the projector of the constraint. */
- virtual dg::Matrix& computeConstraintProjector(dg::Matrix& Proj,
- const int& time );
-
- /*! @} */
-
- public: /* --- DISPLAY --- */
-
- /*! \name Methods to display the stack of tasks.
- @{
- */
- /*! Display the stack of tasks in text mode as a tree. */
- virtual void display( std::ostream& os ) const;
- /*! Wrap the previous method around an operator. */
- SOTSOT_CORE_EXPORT friend std::ostream&
- operator<< (std::ostream& os,const Sot& sot);
- /*! @} */
- public: /* --- SIGNALS --- */
-
- /*! \name Methods to handle signals
- @{
- */
- /*! \brief Intrinsec velocity of the robot, that is used to initialized
- * the recurence of the SOT (e.g. velocity comming from the other
- * OpenHRP plugins).
- */
- SignalPtr<dg::Vector,int> q0SIN;
- /*! \brief A matrix K whose columns are a base of the desired velocity.
- * In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free
- * parameter to be computed.
- */
- SignalPtr<dg::Matrix,int> proj0SIN;
- /*! \brief This signal allow to change the threshold for the
- damped pseudo-inverse on-line */
- SignalPtr<double,int> inversionThresholdSIN;
- /*! \brief Allow to get the result of the Constraint projector. */
- SignalTimeDependent<dg::Matrix,int> constraintSOUT;
- /*! \brief Allow to get the result of the computed control law. */
- SignalTimeDependent<dg::Vector,int> controlSOUT;
- /*! @} */
-
- /*! \brief This method write the priority between tasks in the output stream os. */
- virtual std::ostream & writeGraph(std::ostream & os) const;
- };
- } // namespace sot
+namespace sot {
+
+/*! @ingroup stackoftasks
+ \brief This class implements the Stack of Task.
+ It allows to deal with the priority of the controllers
+ through the shell. The controllers can be either constraints
+ either tasks.
+
+
+*/
+class SOTSOT_CORE_EXPORT Sot : public Entity {
+public:
+ /*! \brief Specify the name of the class entity. */
+ static const std::string CLASS_NAME;
+
+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:
+ /*! \brief This field is a list of controllers
+ managed by the stack of tasks. */
+ StackType stack;
+
+ /*! \brief Defines a type for a list of constraints */
+ typedef std::list<Constraint *> ConstraintListType;
+ /*! \brief This field is a list of constraints
+ managed by the stack of tasks. */
+ ConstraintListType constraintList;
+
+ /*! \brief Defines an interval in the state vector of the robot
+ which is the free flyer. */
+ unsigned int ffJointIdFirst, ffJointIdLast;
+ /*! \brief Defines a default joint. */
+ static const unsigned int FF_JOINT_ID_DEFAULT = 0;
+
+ /* double directionalThreshold; */
+ /* bool useContiInverse; */
+
+ /*! \brief Store the number of joints to be used in the
+ command computed by the stack of tasks. */
+ unsigned int nbJoints;
+
+ /*! \brief Store a pointer to compute the gradient */
+ TaskAbstract *taskGradient;
+
+ // Eigen::MatrixXd<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor>
+ // Proj;
+ /*! Force the recomputation at each step. */
+ bool recomputeEachTime;
+
+public:
+ /*! \brief Threshold to compute the dumped pseudo inverse. */
+ static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
+
+ /* static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
+ /* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
+
+ /*! \brief Number of joints by default. */
+ static dg::Matrix &computeJacobianConstrained(const dg::Matrix &Jac,
+ const dg::Matrix &K,
+ dg::Matrix &JK);
+ static dg::Matrix &computeJacobianConstrained(const TaskAbstract &task,
+ const dg::Matrix &K);
+ static void taskVectorToMlVector(const VectorMultiBound &taskVector,
+ Vector &err);
+
+public:
+ /*! \brief Default constructor */
+ Sot(const std::string &name);
+ ~Sot(void) { /* TODO!! */
+ }
+
+ /*! \name Methods to handle the stack.
+ @{
+ */
+ virtual const StackType &tasks() const { return stack; }
+
+ /*! \brief Push the task in the stack.
+ It has a lowest priority than the previous ones.
+ If this is the first task, then it has the highest
+ priority. */
+ virtual void push(TaskAbstract &task);
+ /*! \brief Pop the task from the stack.
+ This method removes the task with the smallest
+ priority in the task. The other are projected
+ in the null-space of their predecessors. */
+ virtual TaskAbstract &pop(void);
+
+ /*! \brief This method allows to know if a task exists or not */
+ virtual bool exist(const TaskAbstract &task);
+
+ /*! \brief Remove a task regardless to its position in the stack.
+ It removes also the signals connected to the output signal of this
+ stack.*/
+ virtual void remove(const TaskAbstract &task);
+
+ /*! \brief This method removes the output signals depending on
+ this task. */
+ virtual void removeDependency(const TaskAbstract &key);
+
+ /*! \brief This method makes the task to swap with the task having the
+ immediate superior priority. */
+ virtual void up(const TaskAbstract &task);
+
+ /*! \brief This method makes the task to swap with the task having the
+ immediate inferior priority. */
+ virtual void down(const TaskAbstract &task);
+
+ /*! \brief Remove all the tasks from the stack. */
+ virtual void clear(void);
+ /*! @} */
+
+ /*! \name Methods to handle the constraints.
+ @{
+ */
+ /*! \brief Add a constraint to the stack with the current level
+ of priority. */
+ virtual void addConstraint(Constraint &constraint);
+ /*! \brief Remove a constraint from the stack. */
+ virtual void removeConstraint(const Constraint &constraint);
+ /*! \brief Remove all the constraints from the stack. */
+ virtual void clearConstraint(void);
+
+ /*! @} */
+
+ /*! \brief This method defines the part of the state vector
+ which correspond to the free flyer of the robot. */
+ virtual void defineFreeFloatingJoints(const unsigned int &jointIdFirst,
+ const unsigned int &jointIdLast = -1);
+ virtual void defineNbDof(const unsigned int &nbDof);
+ virtual const unsigned int &getNbDof() const { return nbJoints; }
+
+ /*! @} */
+public: /* --- CONTROL --- */
+ /*! \name Methods to compute the control law following the
+ recursive definition of the stack of tasks.
+ @{
+ */
+
+ /*! \brief Compute the control law. */
+ virtual dg::Vector &computeControlLaw(dg::Vector &control, const int &time);
+
+ /*! \brief Compute the projector of the constraint. */
+ virtual dg::Matrix &computeConstraintProjector(dg::Matrix &Proj,
+ const int &time);
+
+ /*! @} */
+
+public: /* --- DISPLAY --- */
+ /*! \name Methods to display the stack of tasks.
+ @{
+ */
+ /*! Display the stack of tasks in text mode as a tree. */
+ virtual void display(std::ostream &os) const;
+ /*! Wrap the previous method around an operator. */
+ SOTSOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const Sot &sot);
+ /*! @} */
+public: /* --- SIGNALS --- */
+ /*! \name Methods to handle signals
+ @{
+ */
+ /*! \brief Intrinsec velocity of the robot, that is used to initialized
+ * the recurence of the SOT (e.g. velocity comming from the other
+ * OpenHRP plugins).
+ */
+ SignalPtr<dg::Vector, int> q0SIN;
+ /*! \brief A matrix K whose columns are a base of the desired velocity.
+ * In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free
+ * parameter to be computed.
+ */
+ SignalPtr<dg::Matrix, int> proj0SIN;
+ /*! \brief This signal allow to change the threshold for the
+ damped pseudo-inverse on-line */
+ SignalPtr<double, int> inversionThresholdSIN;
+ /*! \brief Allow to get the result of the Constraint projector. */
+ SignalTimeDependent<dg::Matrix, int> constraintSOUT;
+ /*! \brief Allow to get the result of the computed control law. */
+ SignalTimeDependent<dg::Vector, int> controlSOUT;
+ /*! @} */
+
+ /*! \brief This method write the priority between tasks in the output stream
+ * os. */
+ virtual std::ostream &writeGraph(std::ostream &os) const;
+};
+} // 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 cbc98b0a..3e21dea7 100644
--- a/include/sot/core/stop-watch.hh
+++ b/include/sot/core/stop-watch.hh
@@ -24,13 +24,12 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
-
#ifndef __sot_core_stopwatch_H__
#define __sot_core_stopwatch_H__
+#include <ctime>
#include <iostream>
#include <map>
-#include <ctime>
#include <sstream>
#ifndef WIN32
@@ -39,19 +38,16 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#endif
// Generic stopwatch exception class
-struct StopwatchException
-{
+struct StopwatchException {
public:
- StopwatchException(std::string error) : error(error) { }
+ 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
+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
// much real time passed
};
@@ -59,11 +55,11 @@ enum StopwatchMode
/**
@brief A class representing a stopwatch.
-
+
@code
Stopwatch swatch();
@endcode
-
+
The Stopwatch class can be used to measure execution time of code,
algorithms, etc., // TODO: he Stopwatch can be initialized in two
time-taking modes, CPU time and real time:
@@ -71,13 +67,13 @@ enum StopwatchMode
@code
swatch.set_mode(REAL_TIME);
@endcode
-
+
CPU time is the time spent by the processor on a certain piece of code,
while real time is the real amount of time taken by a certain piece of
code to execute (i.e. in general if you are doing hard work such as
image or video editing on a different process the measured time will
probably increase).
-
+
How does it work? Basically, one wraps the code to be measured with the
following method calls:
@@ -86,24 +82,24 @@ enum StopwatchMode
// Hic est code
swatch.stop("My astounding algorithm");
@endcode
-
+
A string representing the code ID is provided so that nested portions of
code can be profiled separately:
@code
swatch.start("My astounding algorithm");
-
+
swatch.start("My astounding algorithm - Super smart init");
// Initialization
swatch.stop("My astounding algorithm - Super smart init");
-
+
swatch.start("My astounding algorithm - Main loop");
// Loop
swatch.stop("My astounding algorithm - Main loop");
-
+
swatch.stop("My astounding algorithm");
@endcode
-
+
Note: ID strings can be whatever you like, in the previous example I have
used "My astounding algorithm - *" only to enforce the fact that the
measured code portions are part of My astounding algorithm, but there's no
@@ -116,22 +112,22 @@ enum StopwatchMode
swatch.start("Setup");
// First part of setup
swatch.pause("Setup");
-
+
swatch.start("Main logic");
// Main logic
swatch.stop("Main logic");
-
+
swatch.start("Setup");
// Cleanup (part of the setup)
swatch.stop("Setup");
@endcode
-
+
Finally, to report the results of the measurements just run:
-
+
@code
swatch.report("Code ID");
@endcode
-
+
Thou can also provide an additional std::ostream& parameter to report() to
redirect the logging on a different output. Also, you can use the
get_total/min/max/average_time() methods to get the individual numeric data,
@@ -139,20 +135,19 @@ enum StopwatchMode
implement your own logging syntax.
To report all the measurements:
-
+
@code
swatch.report_all();
@endcode
-
+
Same as above, you can redirect the output by providing a std::ostream&
parameter.
*/
class Stopwatch {
public:
-
/** Constructor */
- Stopwatch(StopwatchMode _mode=NONE);
+ Stopwatch(StopwatchMode _mode = NONE);
/** Destructor */
~Stopwatch();
@@ -179,11 +174,11 @@ public:
void reset_all();
/** Dump the data of a certain performance record */
- void report(std::string perf_name, int precision=2,
- std::ostream& output = std::cout);
+ void report(std::string perf_name, int precision = 2,
+ std::ostream &output = std::cout);
/** Dump the data of all the performance records */
- void report_all(int precision=2, std::ostream& output = std::cout);
+ void report_all(int precision = 2, std::ostream &output = std::cout);
/** Returns total execution time of a certain performance */
long double get_total_time(std::string perf_name);
@@ -215,19 +210,12 @@ public:
long double take_time();
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) {
- }
+ PerformanceData()
+ : 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;
@@ -259,11 +247,10 @@ protected:
/** Pointer to the dynamic structure which holds the collection of performance
data */
- std::map<std::string, PerformanceData >* records_of;
-
+ std::map<std::string, PerformanceData> *records_of;
};
-Stopwatch& getProfiler();
+Stopwatch &getProfiler();
#ifndef WIN32
#pragma GCC visibility pop
diff --git a/include/sot/core/switch.hh b/include/sot/core/switch.hh
index f91f27e9..fa159d25 100644
--- a/include/sot/core/switch.hh
+++ b/include/sot/core/switch.hh
@@ -2,82 +2,78 @@
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
#ifndef __SOT_SWITCH_H__
-# define __SOT_SWITCH_H__
+#define __SOT_SWITCH_H__
+#include <dynamic-graph/command-bind.h>
+#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/entity.h>
-#include <dynamic-graph/signal.h>
+#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
-#include <dynamic-graph/pool.h>
-#include <dynamic-graph/command-bind.h>
-#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/signal.h>
#include <sot/core/config.hh>
#include <sot/core/variadic-op.hh>
namespace dynamicgraph {
- namespace sot {
- /// Switch
- template <typename Value, typename Time = int>
- class SOT_CORE_DLLAPI Switch : public VariadicAbstract<Value,Value,Time>
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
+namespace sot {
+/// Switch
+template <typename Value, typename Time = int>
+class SOT_CORE_DLLAPI Switch : public VariadicAbstract<Value, Value, Time> {
+ DYNAMIC_GRAPH_ENTITY_DECL();
- typedef VariadicAbstract<Value,Value,Time> Base;
+ typedef VariadicAbstract<Value, Value, Time> Base;
- Switch (const std::string& name) :
- Base (name,CLASS_NAME),
- selectionSIN(NULL,"Switch("+name+")::input(int)::selection"),
- boolSelectionSIN(NULL,"Switch("+name+")::input(bool)::boolSelection")
- {
- this->signalRegistration (selectionSIN << boolSelectionSIN);
- this->SOUT.setFunction (boost::bind(&Switch::signal,this,_1,_2));
- this->SOUT.addDependency (selectionSIN);
- this->SOUT.addDependency (boolSelectionSIN);
+ Switch(const std::string &name)
+ : Base(name, CLASS_NAME),
+ selectionSIN(NULL, "Switch(" + name + ")::input(int)::selection"),
+ boolSelectionSIN(NULL,
+ "Switch(" + name + ")::input(bool)::boolSelection") {
+ this->signalRegistration(selectionSIN << boolSelectionSIN);
+ this->SOUT.setFunction(boost::bind(&Switch::signal, this, _1, _2));
+ this->SOUT.addDependency(selectionSIN);
+ this->SOUT.addDependency(boolSelectionSIN);
- using command::makeCommandVoid1;
- std::string docstring =
- "\n"
- " Set number of input signals\n";
- this->addCommand ("setSignalNumber", makeCommandVoid1
- (*(Base*)this, &Base::setSignalNumber, docstring));
+ using command::makeCommandVoid1;
+ 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";
- this->addCommand ("getSignalNumber",
- new command::Getter<Base, int> (*this, &Base::getSignalNumber, docstring));
- }
+ docstring = "\n"
+ " Get number of input signals\n";
+ this->addCommand("getSignalNumber",
+ new command::Getter<Base, int>(
+ *this, &Base::getSignalNumber, docstring));
+ }
- ~Switch () {}
+ ~Switch() {}
- /// Header documentation of the python class
- virtual std::string getDocString () const
- {
- return
- "Dynamically select a given signal based on a input information.\n";
- }
+ /// Header documentation of the python class
+ virtual std::string getDocString() const {
+ return "Dynamically select a given signal based on a input information.\n";
+ }
- private:
- Value& signal (Value& ret, const Time& time)
- {
- int sel;
- if (selectionSIN.isPlugged()) {
- sel = selectionSIN (time);
- } else {
- const bool& b = boolSelectionSIN(time);
- sel = b ? 1 : 0;
- }
- if (sel < 0 || sel >= int(this->signalsIN.size()))
- throw std::runtime_error ("Signal selection is out of range.");
+private:
+ Value &signal(Value &ret, const Time &time) {
+ int sel;
+ if (selectionSIN.isPlugged()) {
+ sel = selectionSIN(time);
+ } else {
+ const bool &b = boolSelectionSIN(time);
+ sel = b ? 1 : 0;
+ }
+ if (sel < 0 || sel >= int(this->signalsIN.size()))
+ throw std::runtime_error("Signal selection is out of range.");
- ret = this->signalsIN[sel]->access (time);
- return ret;
- }
+ ret = this->signalsIN[sel]->access(time);
+ return ret;
+ }
- SignalPtr <int, Time> selectionSIN;
- SignalPtr <bool, Time> boolSelectionSIN;
- };
- } // namespace sot
+ SignalPtr<int, Time> selectionSIN;
+ SignalPtr<bool, Time> boolSelectionSIN;
+};
+} // 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 24a218fa..cb9fe4dd 100644
--- a/include/sot/core/task-abstract.hh
+++ b/include/sot/core/task-abstract.hh
@@ -15,73 +15,72 @@
/* --------------------------------------------------------------------- */
/* Matrix */
-#include <dynamic-graph/linear-algebra.h>
#include <Eigen/SVD>
+#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* STD */
#include <string>
/* SOT */
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
+#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 ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
-
- /// Hierarchical element of the stack of tasks.
- ///
- /// A task computes a value and a Jacobian as output signals.
- /// Once stacked into a solver, the solver will compute the control
- /// vector that makes the task values converge toward zero in the
- /// order defined by the priority levels.
- ///
- /// \image html pictures/task.png "Task diagram: Task types derive from TaskAbstract. The value and Jacobian of a Task are computed from the features that are stored in the task.
-
- class SOT_CORE_EXPORT TaskAbstract
- : public dg::Entity
- {
- public:
-
- /* Use a derivative of this class to store computational memory. */
- class MemoryTaskAbstract
- {
- public:
- int timeLastChange;
- public:
- MemoryTaskAbstract( void ) : timeLastChange(0) {};
- virtual ~MemoryTaskAbstract( void ) {};
- public:
- virtual void display( std::ostream& os ) const = 0;
- friend std::ostream& operator<<( std::ostream& os,
- const MemoryTaskAbstract& tcm )
- {tcm.display(os); return os;}
- };
-
- public:
- MemoryTaskAbstract * memoryInternal;
-
- protected:
- void taskRegistration( void );
-
- public:
- TaskAbstract( const std::string& n );
-
- public: /* --- SIGNALS --- */
-
- dg::SignalTimeDependent< VectorMultiBound,int > taskSOUT;
- dg::SignalTimeDependent< dg::Matrix,int > jacobianSOUT;
- };
-
- } /* namespace sot */
+namespace sot {
+
+/// Hierarchical element of the stack of tasks.
+///
+/// A task computes a value and a Jacobian as output signals.
+/// Once stacked into a solver, the solver will compute the control
+/// vector that makes the task values converge toward zero in the
+/// order defined by the priority levels.
+///
+/// \image html pictures/task.png "Task diagram: Task types derive from
+/// TaskAbstract. The value and Jacobian of a Task are computed from the
+/// features that are stored in the task.
+
+class SOT_CORE_EXPORT TaskAbstract : public dg::Entity {
+public:
+ /* Use a derivative of this class to store computational memory. */
+ class MemoryTaskAbstract {
+ public:
+ int timeLastChange;
+
+ public:
+ MemoryTaskAbstract(void) : timeLastChange(0){};
+ virtual ~MemoryTaskAbstract(void){};
+
+ public:
+ virtual void display(std::ostream &os) const = 0;
+ friend std::ostream &operator<<(std::ostream &os,
+ const MemoryTaskAbstract &tcm) {
+ tcm.display(os);
+ return os;
+ }
+ };
+
+public:
+ MemoryTaskAbstract *memoryInternal;
+
+protected:
+ void taskRegistration(void);
+
+public:
+ TaskAbstract(const std::string &n);
+
+public: /* --- SIGNALS --- */
+ dg::SignalTimeDependent<VectorMultiBound, int> taskSOUT;
+ dg::SignalTimeDependent<dg::Matrix, int> jacobianSOUT;
+};
+
+} /* namespace sot */
} /* namespace dynamicgraph */
-
#endif /* #ifndef __SOT_TASKABSTRACT_H__ */
diff --git a/include/sot/core/task-conti.hh b/include/sot/core/task-conti.hh
index a273f17a..01b71002 100644
--- a/include/sot/core/task-conti.hh
+++ b/include/sot/core/task-conti.hh
@@ -32,63 +32,55 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (task_conti_EXPORTS)
-# define SOTTASKCONTI_EXPORT __declspec(dllexport)
-# else
-# define SOTTASKCONTI_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(task_conti_EXPORTS)
+#define SOTTASKCONTI_EXPORT __declspec(dllexport)
#else
-# define SOTTASKCONTI_EXPORT
+#define SOTTASKCONTI_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTTASKCONTI_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTTASKCONTI_EXPORT TaskConti
-: public Task
-{
- protected:
- enum TimeRefValues
- {
- TIME_REF_UNSIGNIFICANT = -1
- ,TIME_REF_TO_BE_SET =-2
- };
-
+class SOTTASKCONTI_EXPORT TaskConti : public Task {
+protected:
+ enum TimeRefValues { TIME_REF_UNSIGNIFICANT = -1, TIME_REF_TO_BE_SET = -2 };
int timeRef;
double mu;
dg::Vector q0;
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- public:
- TaskConti( const std::string& n );
+public:
+ TaskConti(const std::string &n);
- void referenceTime( const unsigned int & t ) { timeRef = t; }
- const int & referenceTime( void ) { return timeRef; }
+ void referenceTime(const unsigned int &t) { timeRef = t; }
+ const int &referenceTime(void) { return timeRef; }
/* --- COMPUTATION --- */
- VectorMultiBound& computeContiDesiredVelocity( VectorMultiBound &task,
- const int & time );
-
+ VectorMultiBound &computeContiDesiredVelocity(VectorMultiBound &task,
+ const int &time);
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalPtr< dg::Vector,int > controlPrevSIN;
+public:
+ dg::SignalPtr<dg::Vector, int> controlPrevSIN;
/* --- DISPLAY ------------------------------------------------------------ */
- void display( std::ostream& os ) const;
+ void display(std::ostream &os) const;
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASKCONTI_H__ */
diff --git a/include/sot/core/task-pd.hh b/include/sot/core/task-pd.hh
index 4103f136..2778afb4 100644
--- a/include/sot/core/task-pd.hh
+++ b/include/sot/core/task-pd.hh
@@ -14,7 +14,6 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/* SOT */
#include <sot/core/task.hh>
@@ -22,54 +21,49 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (task_pd_EXPORTS)
-# define SOTTASKPD_EXPORT __declspec(dllexport)
-# else
-# define SOTTASKPD_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(task_pd_EXPORTS)
+#define SOTTASKPD_EXPORT __declspec(dllexport)
+#else
+#define SOTTASKPD_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTTASKPD_EXPORT
+#define SOTTASKPD_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTTASKPD_EXPORT TaskPD
-: public Task
-{
- public:
+class SOTTASKPD_EXPORT TaskPD : public Task {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
dg::Vector previousError;
double beta;
- public:
- TaskPD( const std::string& n );
-
+public:
+ TaskPD(const std::string &n);
/* --- COMPUTATION --- */
- dg::Vector& computeErrorDot( dg::Vector& error,int time );
- VectorMultiBound& computeTaskModif( VectorMultiBound& error,int time );
-
+ dg::Vector &computeErrorDot(dg::Vector &error, int time);
+ VectorMultiBound &computeTaskModif(VectorMultiBound &error, int time);
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalTimeDependent< dg::Vector,int > errorDotSOUT;
- dg::SignalPtr< dg::Vector,int > errorDotSIN;
+public:
+ dg::SignalTimeDependent<dg::Vector, int> errorDotSOUT;
+ dg::SignalPtr<dg::Vector, int> errorDotSIN;
/* --- PARAMS --- */
- void initCommand( void );
-
+ void initCommand(void);
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASK_PD_H__ */
diff --git a/include/sot/core/task-unilateral.hh b/include/sot/core/task-unilateral.hh
index a7cae621..071669d6 100644
--- a/include/sot/core/task-unilateral.hh
+++ b/include/sot/core/task-unilateral.hh
@@ -32,54 +32,50 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (task_unilateral_EXPORTS)
-# define SOTTASKUNILATERAL_EXPORT __declspec(dllexport)
-# else
-# define SOTTASKUNILATERAL_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(task_unilateral_EXPORTS)
+#define SOTTASKUNILATERAL_EXPORT __declspec(dllexport)
#else
-# define SOTTASKUNILATERAL_EXPORT
+#define SOTTASKUNILATERAL_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTTASKUNILATERAL_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTTASKUNILATERAL_EXPORT TaskUnilateral
-: public Task
-{
- protected:
- std::list< FeatureAbstract* > featureList;
+class SOTTASKUNILATERAL_EXPORT TaskUnilateral : public Task {
+protected:
+ std::list<FeatureAbstract *> featureList;
- public:
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- public:
- TaskUnilateral( const std::string& n );
+public:
+ TaskUnilateral(const std::string &n);
/* --- COMPUTATION --- */
- VectorMultiBound& computeTaskUnilateral( VectorMultiBound& res,int time );
-
+ VectorMultiBound &computeTaskUnilateral(VectorMultiBound &res, int time);
/* --- SIGNALS ------------------------------------------------------------ */
- public:
-
- dg::SignalPtr< dg::Vector,int > positionSIN;
- dg::SignalPtr< dg::Vector,int > referenceInfSIN;
- dg::SignalPtr< dg::Vector,int > referenceSupSIN;
- dg::SignalPtr< double,int > dtSIN;
+public:
+ dg::SignalPtr<dg::Vector, int> positionSIN;
+ dg::SignalPtr<dg::Vector, int> referenceInfSIN;
+ dg::SignalPtr<dg::Vector, int> referenceSupSIN;
+ dg::SignalPtr<double, int> dtSIN;
/* --- DISPLAY ------------------------------------------------------------ */
- void display( std::ostream& os ) const;
+ void display(std::ostream &os) const;
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASKUNILATERAL_H__ */
diff --git a/include/sot/core/task.hh b/include/sot/core/task.hh
index f93a1c27..24e6e1a7 100644
--- a/include/sot/core/task.hh
+++ b/include/sot/core/task.hh
@@ -32,14 +32,14 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined task_EXPORTS
-# define SOTTASK_EXPORT __declspec(dllexport)
-# else
-# define SOTTASK_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined task_EXPORTS
+#define SOTTASK_EXPORT __declspec(dllexport)
#else
-# define SOTTASK_EXPORT
+#define SOTTASK_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTTASK_EXPORT
#endif
/* --------------------------------------------------------------------- */
@@ -68,61 +68,59 @@ namespace dg = dynamicgraph;
addControlSelection, clearControlSelection.
*/
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTTASK_EXPORT Task
-: public TaskAbstract
-{
- public:
- typedef std::list< FeatureAbstract* > FeatureList_t;
- protected:
+class SOTTASK_EXPORT Task : public TaskAbstract {
+public:
+ typedef std::list<FeatureAbstract *> FeatureList_t;
+
+protected:
FeatureList_t featureList;
bool withDerivative;
DYNAMIC_GRAPH_ENTITY_DECL();
- public:
- Task( const std::string& n );
- void initCommands( void );
+public:
+ Task(const std::string &n);
+ void initCommands(void);
- void addFeature( FeatureAbstract& s );
- void addFeatureFromName( const std::string & name );
- void clearFeatureList( void );
- FeatureList_t & getFeatureList( void ) { return featureList; }
+ void addFeature(FeatureAbstract &s);
+ void addFeatureFromName(const std::string &name);
+ void clearFeatureList(void);
+ FeatureList_t &getFeatureList(void) { return featureList; }
- void setControlSelection( const Flags& act );
- void addControlSelection( const Flags& act );
- void clearControlSelection( void );
+ void setControlSelection(const Flags &act);
+ void addControlSelection(const Flags &act);
+ void clearControlSelection(void);
- void setWithDerivative( const bool & s );
- bool getWithDerivative( void );
+ void setWithDerivative(const bool &s);
+ bool getWithDerivative(void);
/* --- COMPUTATION --- */
- dg::Vector& computeError( dg::Vector& error,int time );
- VectorMultiBound&
- computeTaskExponentialDecrease( VectorMultiBound& errorRef,int time );
- dg::Matrix& computeJacobian( dg::Matrix& J,int time );
- dg::Vector& computeErrorTimeDerivative( dg::Vector & res, int time);
-
+ dg::Vector &computeError(dg::Vector &error, int time);
+ VectorMultiBound &computeTaskExponentialDecrease(VectorMultiBound &errorRef,
+ int time);
+ dg::Matrix &computeJacobian(dg::Matrix &J, int time);
+ dg::Vector &computeErrorTimeDerivative(dg::Vector &res, int time);
/* --- SIGNALS ------------------------------------------------------------ */
- public:
- dg::SignalPtr< double,int > controlGainSIN;
- dg::SignalPtr< double,int > dampingGainSINOUT;
- dg::SignalPtr< Flags,int > controlSelectionSIN;
- dg::SignalTimeDependent< dg::Vector,int > errorSOUT;
- dg::SignalTimeDependent< dg::Vector,int > errorTimeDerivativeSOUT;
+public:
+ dg::SignalPtr<double, int> controlGainSIN;
+ dg::SignalPtr<double, int> dampingGainSINOUT;
+ dg::SignalPtr<Flags, int> controlSelectionSIN;
+ dg::SignalTimeDependent<dg::Vector, int> errorSOUT;
+ dg::SignalTimeDependent<dg::Vector, int> errorTimeDerivativeSOUT;
/* --- DISPLAY ------------------------------------------------------------ */
- void display( std::ostream& os ) const;
+ void display(std::ostream &os) const;
/* --- Writing graph --- */
- virtual std::ostream& writeGraph( std::ostream& os ) const;
+ virtual std::ostream &writeGraph(std::ostream &os) const;
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_TASK_H__ */
diff --git a/include/sot/core/time-stamp.hh b/include/sot/core/time-stamp.hh
index 35f55a48..6c1a9d03 100644
--- a/include/sot/core/time-stamp.hh
+++ b/include/sot/core/time-stamp.hh
@@ -14,7 +14,6 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
@@ -27,73 +26,66 @@ namespace dg = dynamicgraph;
#endif /*WIN32*/
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <sot/core/debug.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (time_stamp_EXPORTS)
-# define TimeStamp_EXPORT __declspec(dllexport)
-# else
-# define TimeStamp_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(time_stamp_EXPORTS)
+#define TimeStamp_EXPORT __declspec(dllexport)
#else
-# define TimeStamp_EXPORT
+#define TimeStamp_EXPORT __declspec(dllimport)
+#endif
+#else
+#define TimeStamp_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class TimeStamp_EXPORT TimeStamp
-:public dg::Entity
-{
- public:
+class TimeStamp_EXPORT TimeStamp : public dg::Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
+protected:
struct timeval val;
unsigned int offsetValue;
bool offsetSet;
- public:
-
+public:
/* --- CONSTRUCTION --- */
- TimeStamp( const std::string& name );
-
- public: /* --- DISPLAY --- */
- virtual void display( std::ostream& os ) const;
+ TimeStamp(const std::string &name);
- public: /* --- SIGNALS --- */
+public: /* --- DISPLAY --- */
+ virtual void display(std::ostream &os) const;
+public: /* --- SIGNALS --- */
/* These signals can be called several time per period, given
* each time a different results. Useful for chronos. */
- dg::Signal<dg::Vector,int> timeSOUT;
- dg::Signal<double,int> timeDoubleSOUT;
+ dg::Signal<dg::Vector, int> timeSOUT;
+ dg::Signal<double, int> timeDoubleSOUT;
/* These signals can be called several time per period, but give
* always the same results different results. Useful for synchro. */
- dg::SignalTimeDependent<dg::Vector,int> timeOnceSOUT;
- dg::SignalTimeDependent<double,int> timeOnceDoubleSOUT;
-
-
- protected: /* --- SIGNAL FUNCTIONS --- */
- dg::Vector& getTimeStamp( dg::Vector& res,const int& time );
- double& getTimeStampDouble( const dg::Vector& vect,double& res );
+ dg::SignalTimeDependent<dg::Vector, int> timeOnceSOUT;
+ dg::SignalTimeDependent<double, int> timeOnceDoubleSOUT;
+protected: /* --- SIGNAL FUNCTIONS --- */
+ dg::Vector &getTimeStamp(dg::Vector &res, const int &time);
+ double &getTimeStampDouble(const dg::Vector &vect, double &res);
};
-
-} /* namespace sot */} /* namespace dynamicgraph */
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif /* #ifndef __SOT_SOT_HH */
diff --git a/include/sot/core/timer.hh b/include/sot/core/timer.hh
index 1a48cbd1..7651c81a 100644
--- a/include/sot/core/timer.hh
+++ b/include/sot/core/timer.hh
@@ -15,33 +15,33 @@
/* --------------------------------------------------------------------- */
/* Classes standards. */
-#include <list> /* Classe std::list */
+#include <list> /* Classe std::list */
#ifndef WIN32
#include <sys/time.h>
-#else /*WIN32*/
+#else /*WIN32*/
// 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>
-#include <Winsock2.h>
#endif /*WIN32*/
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <sot/core/debug.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (timer_EXPORTS)
-# define Timer_EXPORT __declspec(dllexport)
-# else
-# define Timer_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(timer_EXPORTS)
+#define Timer_EXPORT __declspec(dllexport)
#else
-# define Timer_EXPORT
+#define Timer_EXPORT __declspec(dllimport)
+#endif
+#else
+#define Timer_EXPORT
#endif
/* --------------------------------------------------------------------- */
@@ -49,122 +49,105 @@
/* --------------------------------------------------------------------- */
namespace dg = dynamicgraph;
-template< class T >
-class Timer_EXPORT Timer
-:public dg::Entity
-{
- public:
+template <class T> class Timer_EXPORT Timer : public dg::Entity {
+public:
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
-
- protected:
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- struct timeval t0,t1;
+protected:
+ struct timeval t0, t1;
clock_t c0, c1;
double dt;
- public:
-
+public:
/* --- CONSTRUCTION --- */
- Timer( const std::string& name );
-
- public: /* --- DISPLAY --- */
- virtual void display( std::ostream& os ) const;
- Timer_EXPORT friend std::ostream& operator<< ( std::ostream& os,const Timer<T>& timer )
- { timer.display(os); return os; }
-
- public: /* --- SIGNALS --- */
-
- dg::SignalPtr<T,int> sigSIN;
- dg::SignalTimeDependent<T,int> sigSOUT;
- dg::SignalTimeDependent<T,int> sigClockSOUT;
- dg::Signal<double,int> timerSOUT;
-
-
- protected: /* --- SIGNAL FUNCTIONS --- */
- void plug( dg::Signal<T,int> &sig )
- {
- sigSIN = &sig; dt=0.;
+ Timer(const std::string &name);
+
+public: /* --- DISPLAY --- */
+ virtual void display(std::ostream &os) const;
+ Timer_EXPORT friend std::ostream &operator<<(std::ostream &os,
+ const Timer<T> &timer) {
+ timer.display(os);
+ return os;
+ }
+
+public: /* --- SIGNALS --- */
+ dg::SignalPtr<T, int> sigSIN;
+ dg::SignalTimeDependent<T, int> sigSOUT;
+ dg::SignalTimeDependent<T, int> sigClockSOUT;
+ dg::Signal<double, int> timerSOUT;
+
+protected: /* --- SIGNAL FUNCTIONS --- */
+ void plug(dg::Signal<T, int> &sig) {
+ sigSIN = &sig;
+ dt = 0.;
+ }
+
+ template <bool UseClock> T &compute(T &t, const int &time) {
+ sotDEBUGIN(15);
+ if (UseClock) {
+ c0 = clock();
+ sotDEBUG(15) << "t0: " << c0 << std::endl;
+ } else {
+ gettimeofday(&t0, NULL);
+ sotDEBUG(15) << "t0: " << t0.tv_sec << " - " << t0.tv_usec << std::endl;
}
- template <bool UseClock>
- T& compute( T& t,const int& time )
- {
- sotDEBUGIN(15);
- if (UseClock) {
- c0 = clock();
- sotDEBUG(15) << "t0: "<< c0 << std::endl;
- } else {
- gettimeofday(&t0,NULL);
- sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
- }
-
- t = sigSIN( time );
-
- if (UseClock) {
- c1 = clock();
- sotDEBUG(15) << "t1: "<< c0 << std::endl;
- dt = ((double)(c1 - c0) * 1000 ) / CLOCKS_PER_SEC;
- } else {
- gettimeofday(&t1,NULL);
- dt = ( (static_cast<double>(t1.tv_sec)-static_cast<double>(t0.tv_sec)) * 1000.
- + (static_cast<double>(t1.tv_usec)-static_cast<double>(t0.tv_usec)+0.) / 1000. );
- sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
- }
-
- timerSOUT = dt;
- timerSOUT.setTime (time);
-
- sotDEBUGOUT(15);
- return t;
+ t = sigSIN(time);
+
+ if (UseClock) {
+ c1 = clock();
+ sotDEBUG(15) << "t1: " << c0 << std::endl;
+ dt = ((double)(c1 - c0) * 1000) / CLOCKS_PER_SEC;
+ } else {
+ gettimeofday(&t1, NULL);
+ dt = ((static_cast<double>(t1.tv_sec) - static_cast<double>(t0.tv_sec)) *
+ 1000. +
+ (static_cast<double>(t1.tv_usec) - static_cast<double>(t0.tv_usec) +
+ 0.) /
+ 1000.);
+ sotDEBUG(15) << "t1: " << t1.tv_sec << " - " << t1.tv_usec << std::endl;
}
- double& getDt( double& res,const int& /*time*/ )
- {
- res=dt;
- return res;
- }
-};
+ timerSOUT = dt;
+ timerSOUT.setTime(time);
-void cmdChrono( const std::string& cmd,
- std::istringstream& args,
- std::ostream& os );
+ sotDEBUGOUT(15);
+ return t;
+ }
+ double &getDt(double &res, const int & /*time*/) {
+ res = dt;
+ return res;
+ }
+};
+void cmdChrono(const std::string &cmd, std::istringstream &args,
+ std::ostream &os);
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/* --- CONSTRUCTION ---------------------------------------------------- */
-template< class T >
-Timer<T>::
-Timer( const std::string& name )
- :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,
- "Timer("+name+")::output(T)::clockSout" )
- ,timerSOUT( "Timer("+name+")::output(double)::timer" )
-{
+template <class T>
+Timer<T>::Timer(const std::string &name)
+ : 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,
+ "Timer(" + name + ")::output(T)::clockSout"),
+ timerSOUT("Timer(" + name + ")::output(double)::timer") {
sotDEBUGIN(15);
- timerSOUT.setFunction( boost::bind(&Timer::getDt,this,_1,_2) );
+ timerSOUT.setFunction(boost::bind(&Timer::getDt, this, _1, _2));
- signalRegistration( sigSIN<<sigSOUT<<sigClockSOUT<<timerSOUT );
+ signalRegistration(sigSIN << sigSOUT << sigClockSOUT << timerSOUT);
sotDEBUGOUT(15);
}
/* --- DISPLAY --------------------------------------------------------- */
-template< class T >
-void Timer<T>::
-display( std::ostream& os ) const
-{
- os << "Timer <"<< sigSIN << "> : " << dt << "ms." << std::endl;
+template <class T> void Timer<T>::display(std::ostream &os) const {
+ os << "Timer <" << sigSIN << "> : " << dt << "ms." << std::endl;
}
#endif /* #ifndef __SOT_SOT_HH */
diff --git a/include/sot/core/trajectory.hh b/include/sot/core/trajectory.hh
index b384bdaf..7c25d02f 100644
--- a/include/sot/core/trajectory.hh
+++ b/include/sot/core/trajectory.hh
@@ -9,13 +9,12 @@
#ifndef SOT_TRAJECTORY_H__
#define SOT_TRAJECTORY_H__
-
// Matrix
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/api.hh>
-#include <boost/assign/list_of.hpp>
#include <boost/array.hpp>
+#include <boost/assign/list_of.hpp>
#include <boost/regex.hpp>
namespace dg = dynamicgraph;
@@ -26,107 +25,94 @@ namespace sot {
class Trajectory;
-class RulesJointTrajectory
-{
- protected:
- Trajectory & TrajectoryToFill_;
+class RulesJointTrajectory {
+protected:
+ Trajectory &TrajectoryToFill_;
- public:
+public:
unsigned int dbg_level;
/// \brief Strings specifying the grammar of the structure.
- std::string float_str_re,seq_str_re, timestamp_str_re,
- frame_id_str_re, header_str_re, joint_name_str_re,
- list_of_jn_str_re,point_value_str_re, list_of_pv_str_re,
- bg_pt_str_re, end_pt_str_re, comma_pt_str_re, bg_liste_of_pts_str_re
- ;
+ std::string float_str_re, seq_str_re, timestamp_str_re, frame_id_str_re,
+ header_str_re, joint_name_str_re, list_of_jn_str_re, point_value_str_re,
+ list_of_pv_str_re, bg_pt_str_re, end_pt_str_re, comma_pt_str_re,
+ bg_liste_of_pts_str_re;
/// \brief Boost regular expressions implementing the grammar.
- boost::regex header_re,list_of_jn_re,
- list_of_pv_re,
- bg_pt_re, end_pt_re,comma_pt_re, bg_liste_of_pts_re;
+ boost::regex header_re, list_of_jn_re, list_of_pv_re, bg_pt_re, end_pt_re,
+ comma_pt_re, bg_liste_of_pts_re;
std::vector<std::string> joint_names;
- /// \brief Constructor TrajectoryToFill is the structure where to store the parsed information.
+ /// \brief Constructor TrajectoryToFill is the structure where to store the
+ /// parsed information.
RulesJointTrajectory(Trajectory &TrajectoryToFill);
/// \brief parse_string will fill TrajectoryToFill with string atext.
void parse_string(std::string &atext);
- 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);
+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);
/// \brief Find and store the header.
/// This method is looking for:
/// unsigned int seq.
/// unsigned int sec, unsigned int nsec.
/// string format_id
- void parse_header(std::string &text,
- std::string &sub_text1);
+ void parse_header(std::string &text, std::string &sub_text1);
/// \brief Understand joint_names.
/// Extract a list of strings.
- void parse_joint_names(std::string &text,
- std::string &sub_text1,
+ void parse_joint_names(std::string &text, std::string &sub_text1,
std::vector<std::string> &joint_names);
/// \brief Extract a sequence of doubles.
/// To be used for position, velocities, accelerations and effort.
- bool parse_seq(std::string &text,
- std::string &sub_text1,
+ bool parse_seq(std::string &text, std::string &sub_text1,
std::vector<double> &seq);
/// \brief Extract a point description.
- bool parse_point(std::string &trajectory,
- std::string &sub_text1);
+ bool parse_point(std::string &trajectory, std::string &sub_text1);
/// \brief Extract a sequence of points.
- bool parse_points(std::string &trajectory,
- std::string &sub_text1);
-
+ bool parse_points(std::string &trajectory, std::string &sub_text1);
};
-class SOT_CORE_EXPORT timestamp
-{
+class SOT_CORE_EXPORT timestamp {
public:
unsigned long int secs_;
unsigned long int nsecs_;
- timestamp() : secs_(0),nsecs_(0)
- {}
- timestamp(const timestamp &ats)
- { secs_ = ats.secs_; nsecs_ = ats.nsecs_; }
- timestamp(unsigned long int lsecs,
- unsigned long int lnsecs)
- { secs_ = lsecs; nsecs_ = lnsecs;}
- bool operator==(const timestamp &other) const
- {
- if ((secs_!=other.secs_) || (nsecs_!=other.nsecs_))
+ timestamp() : secs_(0), nsecs_(0) {}
+ timestamp(const timestamp &ats) {
+ secs_ = ats.secs_;
+ nsecs_ = ats.nsecs_;
+ }
+ timestamp(unsigned long int lsecs, unsigned long int lnsecs) {
+ secs_ = lsecs;
+ nsecs_ = lnsecs;
+ }
+ bool operator==(const timestamp &other) const {
+ if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_))
return false;
return true;
}
- friend std::ostream& operator <<(std::ostream& stream, const timestamp& ats)
- {
- stream << ats.secs_ + 0.000001*(long double)ats.nsecs_;
+ friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
+ stream << ats.secs_ + 0.000001 * (long double)ats.nsecs_;
return stream;
}
};
-class SOT_CORE_EXPORT Header
-{
+class SOT_CORE_EXPORT Header {
public:
unsigned int seq_;
timestamp stamp_;
std::string frame_id_;
- Header(): seq_(0),stamp_(0,0),frame_id_("initial_trajectory")
- {}
+ Header() : seq_(0), stamp_(0, 0), frame_id_("initial_trajectory") {}
};
-
-class SOT_CORE_EXPORT JointTrajectoryPoint
-{
+class SOT_CORE_EXPORT JointTrajectoryPoint {
public:
std::vector<double> positions_;
@@ -136,66 +122,63 @@ public:
typedef std::vector<double> vec_ref;
- void display(std::ostream &os) const
- {
- boost::array<std::string, 4> names=
- ba::list_of("Positions")("Velocities")("Accelerations")("Effort");
-
- const std::vector<double> *points=0;
-
- 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);
- }
+ void display(std::ostream &os) const {
+ boost::array<std::string, 4> names =
+ ba::list_of("Positions")("Velocities")("Accelerations")("Effort");
- std::vector<double>::const_iterator it_db;
- os << names[arrayId] << std::endl
- << "---------" << std::endl;
- for(it_db = points->begin();
- it_db != points->end();
- it_db++)
- {
- os << *it_db << std::endl;
- }
- }
- }
+ const std::vector<double> *points = 0;
- void transfer(const std::vector<double> &src, unsigned int vecId)
- {
- switch(vecId)
- {
- case(0): positions_ = src;
+ for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
+ switch (arrayId) {
+ case (0):
+ points = &positions_;
break;
- case(1): velocities_ = src;
+ case (1):
+ points = &velocities_;
break;
- case(2): accelerations_ = src;
+ case (2):
+ points = &accelerations_;
break;
- case(3): efforts_ =src;
+ case (3):
+ points = &efforts_;
break;
default:
assert(0);
+ }
+
+ std::vector<double>::const_iterator it_db;
+ os << names[arrayId] << std::endl << "---------" << std::endl;
+ for (it_db = points->begin(); it_db != points->end(); it_db++) {
+ os << *it_db << std::endl;
+ }
+ }
+ }
+
+ 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);
}
}
};
-class SOT_CORE_EXPORT Trajectory
-{
+class SOT_CORE_EXPORT Trajectory {
public:
-
- Trajectory( );
- Trajectory( const Trajectory & copy );
+ Trajectory();
+ Trajectory(const Trajectory ©);
virtual ~Trajectory();
std::vector<std::string> joint_names_;
@@ -207,10 +190,8 @@ public:
int deserialize(std::istringstream &is);
void display(std::ostream &) const;
-
};
} // namespace sot
} // namespace dynamicgraph
-
#endif /* #ifndef SOT_TRAJECTORY_H__ */
diff --git a/include/sot/core/unary-op.hh b/include/sot/core/unary-op.hh
index 705f766a..650a5c94 100644
--- a/include/sot/core/unary-op.hh
+++ b/include/sot/core/unary-op.hh
@@ -15,70 +15,57 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
-
+#include <dynamic-graph/entity.h>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
-
- 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 --- */
-
- static std::string getTypeInName( void ) { return Operator::nameTypeIn(); }
- static std::string getTypeOutName( void ) { return Operator::nameTypeOut(); }
- static const std::string CLASS_NAME;
-
- virtual const std::string& getClassName () const
- {
- return CLASS_NAME;
- }
-
- std::string getDocString () const { return op.getDocString ();}
-
- UnaryOp( const std::string& name )
- : 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")
- {
- signalRegistration( SIN<<SOUT );
- op.addSpecificCommands(*this,commandMap);
- }
-
- virtual ~UnaryOp( void ) {};
-
- public: /* --- SIGNAL --- */
-
- SignalPtr<Tin,int> SIN;
- SignalTimeDependent<Tout,int> SOUT;
-
- protected:
- Tout& computeOperation( Tout& res,int time )
- {
- const Tin &x1 = SIN(time);
- op(x1,res);
- return res;
- }
-
- public: /* --- PARAMS --- */
-
- };
- } /* namespace sot */
+namespace sot {
+
+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 --- */
+ static std::string getTypeInName(void) { return Operator::nameTypeIn(); }
+ static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
+ static const std::string CLASS_NAME;
+
+ virtual const std::string &getClassName() const { return CLASS_NAME; }
+
+ std::string getDocString() const { return op.getDocString(); }
+
+ UnaryOp(const std::string &name)
+ : 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") {
+ signalRegistration(SIN << SOUT);
+ op.addSpecificCommands(*this, commandMap);
+ }
+
+ virtual ~UnaryOp(void){};
+
+public: /* --- SIGNAL --- */
+ SignalPtr<Tin, int> SIN;
+ SignalTimeDependent<Tout, int> SOUT;
+
+protected:
+ Tout &computeOperation(Tout &res, int time) {
+ const Tin &x1 = SIN(time);
+ op(x1, res);
+ return res;
+ }
+
+public: /* --- PARAMS --- */
+};
+} /* namespace sot */
} /* namespace dynamicgraph */
#endif // #ifndef SOT_CORE_UNARYOP_HH
diff --git a/include/sot/core/utils-windows.hh b/include/sot/core/utils-windows.hh
index 879cf0c9..c891b9a8 100644
--- a/include/sot/core/utils-windows.hh
+++ b/include/sot/core/utils-windows.hh
@@ -18,10 +18,9 @@
#define NOMINMAX
#include <Winsock2.h>
-struct SOT_CORE_EXPORT timezone
-{
- int tz_minuteswest; /* minutes W of Greenwich */
- int tz_dsttime; /* type of dst correction */
+struct SOT_CORE_EXPORT timezone {
+ int tz_minuteswest; /* minutes W of Greenwich */
+ int tz_dsttime; /* type of dst correction */
};
int SOT_CORE_EXPORT gettimeofday(struct timeval *tv, struct timezone *tz);
diff --git a/include/sot/core/variadic-op.hh b/include/sot/core/variadic-op.hh
index 807b5e13..77331787 100644
--- a/include/sot/core/variadic-op.hh
+++ b/include/sot/core/variadic-op.hh
@@ -17,11 +17,11 @@
#include <dynamic-graph/linear-algebra.h>
/* SOT */
-#include <sot/core/flags.hh>
-#include <dynamic-graph/entity.h>
-#include <sot/core/pool.hh>
#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>
@@ -29,158 +29,140 @@
#include <boost/function.hpp>
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- template< typename Tin, typename Tout, typename Time >
- class VariadicAbstract
- :public Entity
- {
- 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")
- ,baseSigname(className+"("+name+")::input("+getTypeInName()+")::")
- {
- signalRegistration( SOUT );
- }
-
- virtual ~VariadicAbstract( void )
- {
- for (std::size_t i = 0; i < signalsIN.size(); ++i) {
- _removeSignal (i);
- }
- };
-
- public: /* --- SIGNAL --- */
-
- SignalTimeDependent<Tout,int> SOUT;
-
- std::size_t addSignal ()
- {
- std::ostringstream oss; oss << "sin" << signalsIN.size();
- return addSignal (oss.str());
- }
-
- std::size_t addSignal (const std::string& name)
- {
- signal_t* sig = new signal_t (NULL, baseSigname + name);
- try {
- _declareSignal(sig);
- signalsIN.push_back (sig);
- // names.push_back (name);
- return signalsIN.size()-1;
- } catch (const ExceptionAbstract&) {
- delete sig;
- throw;
- }
- }
-
- void removeSignal ()
- {
- assert (signalsIN.size()>0);
- _removeSignal (signalsIN.size()-1);
- // names.pop_back();
- signalsIN.pop_back();
- }
-
- 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);
- signalsIN.resize(n,NULL);
- // names.resize(n);
-
- for (std::size_t i = oldSize; i < (std::size_t)n; ++i)
- {
- assert (signalsIN[i]==NULL);
- std::ostringstream oss;
- oss << baseSigname << "sin" << i;
- // names[i] = oss.str();
- // signal_t* s = new signal_t (NULL,names[i]);
- signal_t* s = new signal_t (NULL,oss.str());
- signalsIN[i] = s;
- _declareSignal (s);
- }
- }
-
- int getSignalNumber () const
- {
- return (int)signalsIN.size();
- }
-
- protected:
- typedef SignalPtr<Tin,int> signal_t;
- std::vector< signal_t* > signalsIN;
- // Use signal->shortName instead
- // std::vector< std::string > names;
-
- private:
- void _removeSignal (const std::size_t i)
- {
- // signalDeregistration(names[i]);
- signalDeregistration(signalsIN[i]->shortName());
- SOUT.removeDependency (*signalsIN[i]);
- delete signalsIN[i];
- }
- void _declareSignal (signal_t* s)
- {
- signalRegistration(*s);
- SOUT.addDependency (*s);
- }
- const std::string baseSigname;
- };
-
- template< typename Operator >
- class VariadicOp
- :public VariadicAbstract<typename Operator::Tin, typename Operator::Tout, int>
- {
- Operator op;
- typedef typename Operator::Tin Tin;
- typedef typename Operator::Tout Tout;
- typedef VariadicOp<Operator> Self;
-
- public: /* --- CONSTRUCTION --- */
- typedef VariadicAbstract<Tin,Tout,int> Base;
-
- // static std::string getTypeInName ( void ) { return Operator::nameTypeIn (); }
- // static std::string getTypeOutName( void ) { return Operator::nameTypeOut(); }
- static const std::string CLASS_NAME;
- virtual const std::string& getClassName () const { return CLASS_NAME; }
- std::string getDocString () const { return op.getDocString ();}
-
- VariadicOp( const std::string& name )
- : Base(name, CLASS_NAME)
- {
- this->SOUT.setFunction (boost::bind(&Self::computeOperation,this,_1,_2));
- op.initialize(this,this->commandMap);
- }
-
- virtual ~VariadicOp( void ) {};
-
- 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) {
- const Tin& x = this->signalsIN[i]->access (time);
- in[i] = &x;
- }
- op(in,res);
- return res;
- }
- };
- } // namespace sot
-} // namespace dynamicgraph
+namespace sot {
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+template <typename Tin, typename Tout, typename Time>
+class VariadicAbstract : public Entity {
+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"),
+ baseSigname(className + "(" + name + ")::input(" + getTypeInName() +
+ ")::") {
+ signalRegistration(SOUT);
+ }
+
+ virtual ~VariadicAbstract(void) {
+ for (std::size_t i = 0; i < signalsIN.size(); ++i) {
+ _removeSignal(i);
+ }
+ };
+
+public: /* --- SIGNAL --- */
+ SignalTimeDependent<Tout, int> SOUT;
+
+ std::size_t addSignal() {
+ std::ostringstream oss;
+ oss << "sin" << signalsIN.size();
+ return addSignal(oss.str());
+ }
+
+ std::size_t addSignal(const std::string &name) {
+ signal_t *sig = new signal_t(NULL, baseSigname + name);
+ try {
+ _declareSignal(sig);
+ signalsIN.push_back(sig);
+ // names.push_back (name);
+ return signalsIN.size() - 1;
+ } catch (const ExceptionAbstract &) {
+ delete sig;
+ throw;
+ }
+ }
+
+ void removeSignal() {
+ assert(signalsIN.size() > 0);
+ _removeSignal(signalsIN.size() - 1);
+ // names.pop_back();
+ signalsIN.pop_back();
+ }
+
+ 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);
+ signalsIN.resize(n, NULL);
+ // names.resize(n);
+
+ for (std::size_t i = oldSize; i < (std::size_t)n; ++i) {
+ assert(signalsIN[i] == NULL);
+ std::ostringstream oss;
+ oss << baseSigname << "sin" << i;
+ // names[i] = oss.str();
+ // signal_t* s = new signal_t (NULL,names[i]);
+ signal_t *s = new signal_t(NULL, oss.str());
+ signalsIN[i] = s;
+ _declareSignal(s);
+ }
+ }
+
+ int getSignalNumber() const { return (int)signalsIN.size(); }
+
+protected:
+ typedef SignalPtr<Tin, int> signal_t;
+ std::vector<signal_t *> signalsIN;
+ // Use signal->shortName instead
+ // std::vector< std::string > names;
+
+private:
+ void _removeSignal(const std::size_t i) {
+ // signalDeregistration(names[i]);
+ signalDeregistration(signalsIN[i]->shortName());
+ SOUT.removeDependency(*signalsIN[i]);
+ delete signalsIN[i];
+ }
+ void _declareSignal(signal_t *s) {
+ signalRegistration(*s);
+ SOUT.addDependency(*s);
+ }
+ const std::string baseSigname;
+};
+
+template <typename Operator>
+class VariadicOp : public VariadicAbstract<typename Operator::Tin,
+ typename Operator::Tout, int> {
+ Operator op;
+ typedef typename Operator::Tin Tin;
+ typedef typename Operator::Tout Tout;
+ typedef VariadicOp<Operator> Self;
+
+public: /* --- CONSTRUCTION --- */
+ typedef VariadicAbstract<Tin, Tout, int> Base;
+
+ // static std::string getTypeInName ( void ) { return Operator::nameTypeIn ();
+ // } static std::string getTypeOutName( void ) { return
+ // Operator::nameTypeOut(); }
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName() const { return CLASS_NAME; }
+ std::string getDocString() const { return op.getDocString(); }
+
+ VariadicOp(const std::string &name) : Base(name, CLASS_NAME) {
+ this->SOUT.setFunction(boost::bind(&Self::computeOperation, this, _1, _2));
+ op.initialize(this, this->commandMap);
+ }
+
+ virtual ~VariadicOp(void){};
+
+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) {
+ const Tin &x = this->signalsIN[i]->access(time);
+ in[i] = &x;
+ }
+ op(in, res);
+ return res;
+ }
+};
+} // namespace sot
+} // namespace dynamicgraph
#endif // #ifndef SOT_CORE_VARIADICOP_HH
diff --git a/include/sot/core/vector-constant.hh b/include/sot/core/vector-constant.hh
index 32d20694..45c47b95 100644
--- a/include/sot/core/vector-constant.hh
+++ b/include/sot/core/vector-constant.hh
@@ -22,36 +22,34 @@ namespace dg = dynamicgraph;
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace dynamicgraph {
- namespace sot {
+namespace sot {
- namespace command {
- namespace vectorConstant {
- class Resize;
- }
- }
+namespace command {
+namespace vectorConstant {
+class Resize;
+}
+} // namespace command
- class VectorConstant
- : public Entity
- {
- friend class command::vectorConstant::Resize;
- static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+class VectorConstant : public Entity {
+ friend class command::vectorConstant::Resize;
+ static const std::string CLASS_NAME;
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- int rows;
- double color;
+ int rows;
+ double color;
- public:
- VectorConstant( const std::string& name );
+public:
+ VectorConstant(const std::string &name);
- virtual ~VectorConstant( void ){}
+ virtual ~VectorConstant(void) {}
- SignalTimeDependent<dg::Vector,int> SOUT;
+ SignalTimeDependent<dg::Vector, int> SOUT;
- /// \brief Set value of vector (and therefore of output signal)
- void setValue(const dg::Vector& inValue);
- };
+ /// \brief Set value of vector (and therefore of output signal)
+ void setValue(const dg::Vector &inValue);
+};
- } // namespace sot
+} // 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 f7917521..53e79bf5 100644
--- a/include/sot/core/vector-to-rotation.hh
+++ b/include/sot/core/vector-to-rotation.hh
@@ -10,9 +10,8 @@
#ifndef __SOTVECTORTOMATRIX_HH
#define __SOTVECTORTOMATRIX_HH
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/entity.h>
#include <sot/core/matrix-geometry.hh>
/* Matrix */
@@ -26,54 +25,45 @@ namespace dg = dynamicgraph;
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (vector_to_rotation_EXPORTS)
-# define SOTVECTORTOROTATION_EXPORT __declspec(dllexport)
-# else
-# define SOTVECTORTOROTATION_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(vector_to_rotation_EXPORTS)
+#define SOTVECTORTOROTATION_EXPORT __declspec(dllexport)
+#else
+#define SOTVECTORTOROTATION_EXPORT __declspec(dllimport)
+#endif
#else
-# define SOTVECTORTOROTATION_EXPORT
+#define SOTVECTORTOROTATION_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
namespace dg = dynamicgraph;
-class SOTVECTORTOROTATION_EXPORT VectorToRotation
-: public dg::Entity
-{
+class SOTVECTORTOROTATION_EXPORT VectorToRotation : public dg::Entity {
static const std::string CLASS_NAME;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+ virtual const std::string &getClassName(void) const { return CLASS_NAME; }
- enum sotAxis
- {
- AXIS_X
- ,AXIS_Y
- ,AXIS_Z
- };
+ enum sotAxis { AXIS_X, AXIS_Y, AXIS_Z };
unsigned int size;
- std::vector< sotAxis > axes;
-
+ std::vector<sotAxis> axes;
public:
- VectorToRotation( const std::string& name );
-
- virtual ~VectorToRotation( void ){}
+ VectorToRotation(const std::string &name);
- dg::SignalPtr<dg::Vector,int> SIN;
- dg::SignalTimeDependent<MatrixRotation,int> SOUT;
+ virtual ~VectorToRotation(void) {}
- MatrixRotation& computeRotation( const dg::Vector& angles,
- MatrixRotation& res );
+ dg::SignalPtr<dg::Vector, int> SIN;
+ dg::SignalTimeDependent<MatrixRotation, int> SOUT;
+ MatrixRotation &computeRotation(const dg::Vector &angles,
+ MatrixRotation &res);
};
-} /* namespace sot */} /* namespace dynamicgraph */
-
-
+} /* namespace sot */
+} /* namespace dynamicgraph */
#endif // #ifndef __SOTVECTORTOMATRIX_HH
diff --git a/include/sot/core/visual-point-projecter.hh b/include/sot/core/visual-point-projecter.hh
index d8ea78b7..186aa203 100644
--- a/include/sot/core/visual-point-projecter.hh
+++ b/include/sot/core/visual-point-projecter.hh
@@ -9,14 +9,14 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#if defined (WIN32)
-# if defined (visual_point_projecter_EXPORTS)
-# define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllexport)
-# else
-# define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllimport)
-# endif
+#if defined(WIN32)
+#if defined(visual_point_projecter_EXPORTS)
+#define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllexport)
#else
-# define SOTVISUALPOINTPROJECTER_EXPORT
+#define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTVISUALPOINTPROJECTER_EXPORT
#endif
/* --------------------------------------------------------------------- */
@@ -29,44 +29,39 @@ namespace dg = dynamicgraph;
#include <sot/core/matrix-geometry.hh>
/* SOT */
-#include <dynamic-graph/signal-helper.h>
#include <dynamic-graph/entity-helper.h>
+#include <dynamic-graph/signal-helper.h>
namespace dynamicgraph {
- namespace sot {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
+namespace sot {
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
- class SOTVISUALPOINTPROJECTER_EXPORT VisualPointProjecter
- :public ::dynamicgraph::Entity
- ,public ::dynamicgraph::EntityHelper<VisualPointProjecter>
- {
-
- public: /* --- CONSTRUCTOR ---- */
-
- VisualPointProjecter( const std::string & name );
-
- public: /* --- ENTITY INHERITANCE --- */
+class SOTVISUALPOINTPROJECTER_EXPORT VisualPointProjecter
+ : public ::dynamicgraph::Entity,
+ public ::dynamicgraph::EntityHelper<VisualPointProjecter> {
- static const std::string CLASS_NAME;
- virtual void display( std::ostream& os ) const;
- virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+public: /* --- CONSTRUCTOR ---- */
+ VisualPointProjecter(const std::string &name);
- public: /* --- SIGNALS --- */
+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; }
- DECLARE_SIGNAL_IN(point3D,dg::Vector);
- DECLARE_SIGNAL_IN(transfo,MatrixHomogeneous);
+public: /* --- SIGNALS --- */
+ DECLARE_SIGNAL_IN(point3D, dg::Vector);
+ DECLARE_SIGNAL_IN(transfo, MatrixHomogeneous);
- DECLARE_SIGNAL_OUT(point3Dgaze,dg::Vector);
- DECLARE_SIGNAL_OUT(depth,double);
- DECLARE_SIGNAL_OUT(point2D,dg::Vector);
+ DECLARE_SIGNAL_OUT(point3Dgaze, dg::Vector);
+ DECLARE_SIGNAL_OUT(depth, double);
+ DECLARE_SIGNAL_OUT(point2D, dg::Vector);
- }; // class VisualPointProjecter
+}; // class VisualPointProjecter
- } // namespace sot
+} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_core_VisualPointProjecter_H__
diff --git a/src/control/control-gr.cpp b/src/control/control-gr.cpp
index 0ff637ee..dbb7a1e5 100644
--- a/src/control/control-gr.cpp
+++ b/src/control/control-gr.cpp
@@ -10,56 +10,50 @@
/* SOT */
#include <sot/core/debug.hh>
-class ControlGR__INIT
-{
+class ControlGR__INIT {
public:
- ControlGR__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
+ ControlGR__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
ControlGR__INIT ControlGR_initiator;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include <sot/core/control-gr.hh>
-#include <sot/core/binary-op.hh>
#include <dynamic-graph/factory.h>
+#include <sot/core/binary-op.hh>
+#include <sot/core/control-gr.hh>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlGR,"ControlGR");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlGR, "ControlGR");
-const double ControlGR::
-TIME_STEP_DEFAULT = .001;
+const double ControlGR::TIME_STEP_DEFAULT = .001;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#define __SOT_ControlGR_INIT \
-
-ControlGR::
-ControlGR( const std::string & name )
- :Entity(name)
- ,TimeStep(0)
- ,matrixASIN(NULL,"ControlGR("+name+")::input(matrix)::matrixA")
- ,accelerationSIN(NULL,"ControlGR("+name+")::input(vector)::acceleration")
- ,gravitySIN(NULL,"ControlGR("+name+")::input(vector)::gravity")
- ,controlSOUT( boost::bind(&ControlGR::computeControl,this,_1,_2),
- matrixASIN << accelerationSIN << gravitySIN,
- "ControlGR("+name+")::output(vector)::control" )
-{
+#define __SOT_ControlGR_INIT
+
+ControlGR::ControlGR(const std::string &name)
+ : Entity(name), TimeStep(0),
+ matrixASIN(NULL, "ControlGR(" + name + ")::input(matrix)::matrixA"),
+ accelerationSIN(NULL,
+ "ControlGR(" + name + ")::input(vector)::acceleration"),
+ gravitySIN(NULL, "ControlGR(" + name + ")::input(vector)::gravity"),
+ controlSOUT(boost::bind(&ControlGR::computeControl, this, _1, _2),
+ matrixASIN << accelerationSIN << gravitySIN,
+ "ControlGR(" + name + ")::output(vector)::control") {
init(TimeStep);
- Entity::signalRegistration( matrixASIN << accelerationSIN << gravitySIN << controlSOUT );
+ Entity::signalRegistration(matrixASIN << accelerationSIN << gravitySIN
+ << controlSOUT);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void ControlGR::
-init(const double& Stept)
-{
+void ControlGR::init(const double &Stept) {
TimeStep = Stept;
return;
@@ -69,63 +63,59 @@ init(const double& Stept)
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void ControlGR::
-display( std::ostream& os ) const
-{
- os << "ControlGR "<<getName();
- try{
- os <<"control = "<<controlSOUT;
+void ControlGR::display(std::ostream &os) const {
+ os << "ControlGR " << getName();
+ try {
+ os << "control = " << controlSOUT;
+ } catch (ExceptionSignal e) {
}
- catch (ExceptionSignal e) {}
- os <<" ("<<TimeStep<<") ";
+ os << " (" << TimeStep << ") ";
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-double& ControlGR::setsize(int dimension)
+double &ControlGR::setsize(int dimension)
{
- _dimension = dimension;
- return _dimension;
+ _dimension = dimension;
+ return _dimension;
}
-dynamicgraph::Vector& ControlGR::
-computeControl( dynamicgraph::Vector &tau, int t )
-{
+dynamicgraph::Vector &ControlGR::computeControl(dynamicgraph::Vector &tau,
+ int t) {
sotDEBUGIN(15);
- const dynamicgraph::Matrix& matrixA = matrixASIN(t);
- const dynamicgraph::Vector& acceleration = accelerationSIN(t);
- const dynamicgraph::Vector& gravity = gravitySIN(t);
+ const dynamicgraph::Matrix &matrixA = matrixASIN(t);
+ const dynamicgraph::Vector &acceleration = accelerationSIN(t);
+ const dynamicgraph::Vector &gravity = gravitySIN(t);
dynamicgraph::Vector::Index size = acceleration.size();
tau.resize(size);
- //tau*=0;
- /* for(unsigned i = 0u; i < size; ++i)
- {
- tp = gravity(i);
- tau(i) = acceleration;
- tau(i) *= matrixA;
- tau(i) += tp;
- }*/
-
-tau = matrixA*acceleration;
-sotDEBUG(15) << "torque = A*ddot(q)= " << matrixA*acceleration << std::endl;
-tau += gravity;
-
-/*
-tau(1) *= 0;
-tau(7) *= 0;
-tau(24) *=0;
-tau(17)=0;*/
-
- sotDEBUG(15) << "matrixA =" << matrixA << std::endl;
- sotDEBUG(15) << "acceleration =" << acceleration << std::endl;
- sotDEBUG(15) << "gravity =" << gravity << std::endl;
- sotDEBUG(15) << "gravity compensation torque =" << tau << std::endl;
- sotDEBUGOUT(15);
+ // tau*=0;
+ /* for(unsigned i = 0u; i < size; ++i)
+ {
+ tp = gravity(i);
+ tau(i) = acceleration;
+ tau(i) *= matrixA;
+ tau(i) += tp;
+ }*/
+
+ tau = matrixA * acceleration;
+ sotDEBUG(15) << "torque = A*ddot(q)= " << matrixA * acceleration << std::endl;
+ tau += gravity;
+
+ /*
+ tau(1) *= 0;
+ tau(7) *= 0;
+ tau(24) *=0;
+ tau(17)=0;*/
+
+ sotDEBUG(15) << "matrixA =" << matrixA << std::endl;
+ sotDEBUG(15) << "acceleration =" << acceleration << std::endl;
+ sotDEBUG(15) << "gravity =" << gravity << std::endl;
+ sotDEBUG(15) << "gravity compensation torque =" << tau << std::endl;
+ sotDEBUGOUT(15);
return tau;
-
}
diff --git a/src/control/control-pd.cpp b/src/control/control-pd.cpp
index 08c0ff6b..eb57a6b7 100644
--- a/src/control/control-pd.cpp
+++ b/src/control/control-pd.cpp
@@ -13,53 +13,50 @@
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
+#include <sot/core/debug.hh>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlPD, "ControlPD");
-const double ControlPD::
-TIME_STEP_DEFAULT = .001;
+const double ControlPD::TIME_STEP_DEFAULT = .001;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-#define __SOT_ControlPD_INIT \
-
-ControlPD::ControlPD( const std::string & name )
- :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")
- ,velocitySIN(NULL,"ControlPD("+name+")::input(vector)::velocity")
- ,desiredvelocitySIN(NULL,"ControlPD("+name+")::input(vector)::desired_velocity")
- ,controlSOUT( boost::bind(&ControlPD::computeControl,this,_1,_2),
- KpSIN << KdSIN << positionSIN << desiredpositionSIN
- << velocitySIN << desiredvelocitySIN,
- "ControlPD("+name+")::output(vector)::control" )
- ,positionErrorSOUT(boost::bind(&ControlPD::getPositionError,this,_1,_2),
- controlSOUT,
- "ControlPD("+name+")::output(vector)::position_error")
- ,velocityErrorSOUT(boost::bind(&ControlPD::getVelocityError,this,_1,_2),
- controlSOUT,
- "ControlPD("+name+")::output(vector)::velocity_error")
-{
+#define __SOT_ControlPD_INIT
+
+ControlPD::ControlPD(const std::string &name)
+ : 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"),
+ velocitySIN(NULL, "ControlPD(" + name + ")::input(vector)::velocity"),
+ desiredvelocitySIN(NULL, "ControlPD(" + name +
+ ")::input(vector)::desired_velocity"),
+ controlSOUT(boost::bind(&ControlPD::computeControl, this, _1, _2),
+ KpSIN << KdSIN << positionSIN << desiredpositionSIN
+ << velocitySIN << desiredvelocitySIN,
+ "ControlPD(" + name + ")::output(vector)::control"),
+ positionErrorSOUT(
+ boost::bind(&ControlPD::getPositionError, this, _1, _2), controlSOUT,
+ "ControlPD(" + name + ")::output(vector)::position_error"),
+ velocityErrorSOUT(
+ boost::bind(&ControlPD::getVelocityError, this, _1, _2), controlSOUT,
+ "ControlPD(" + name + ")::output(vector)::velocity_error") {
init(TimeStep);
- Entity::signalRegistration( KpSIN << KdSIN << positionSIN <<
- desiredpositionSIN << velocitySIN << desiredvelocitySIN << controlSOUT
- << positionErrorSOUT << velocityErrorSOUT );
+ Entity::signalRegistration(KpSIN << KdSIN << positionSIN << desiredpositionSIN
+ << velocitySIN << desiredvelocitySIN
+ << controlSOUT << positionErrorSOUT
+ << velocityErrorSOUT);
}
-void ControlPD::
-init(const double& Stept)
-{
+void ControlPD::init(const double &Stept) {
TimeStep = Stept;
return;
@@ -69,60 +66,54 @@ init(const double& Stept)
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void ControlPD::display( std::ostream& os ) const
-{
- os << "ControlPD "<<getName();
- try{
- os <<"control = "<<controlSOUT;
+void ControlPD::display(std::ostream &os) const {
+ os << "ControlPD " << getName();
+ try {
+ os << "control = " << controlSOUT;
+ } catch (ExceptionSignal e) {
}
- catch (ExceptionSignal e) {}
- os <<" ("<<TimeStep<<") ";
+ os << " (" << TimeStep << ") ";
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-dynamicgraph::Vector& ControlPD::
-computeControl( dynamicgraph::Vector &tau, int t )
-{
- sotDEBUGIN(15);
- const dynamicgraph::Vector& Kp = KpSIN(t);
- const dynamicgraph::Vector& Kd = KdSIN(t);
- const dynamicgraph::Vector& position = positionSIN(t);
- const dynamicgraph::Vector& desired_position = desiredpositionSIN(t);
- const dynamicgraph::Vector& velocity = velocitySIN(t);
- const dynamicgraph::Vector& desired_velocity = desiredvelocitySIN(t);
-
- dynamicgraph::Vector::Index size = Kp.size();
+dynamicgraph::Vector &ControlPD::computeControl(dynamicgraph::Vector &tau,
+ int t) {
+ sotDEBUGIN(15);
+ const dynamicgraph::Vector &Kp = KpSIN(t);
+ const dynamicgraph::Vector &Kd = KdSIN(t);
+ const dynamicgraph::Vector &position = positionSIN(t);
+ const dynamicgraph::Vector &desired_position = desiredpositionSIN(t);
+ const dynamicgraph::Vector &velocity = velocitySIN(t);
+ const dynamicgraph::Vector &desired_velocity = desiredvelocitySIN(t);
+
+ dynamicgraph::Vector::Index size = Kp.size();
tau.resize(size);
position_error_.resize(size);
velocity_error_.resize(size);
- position_error_.array() = desired_position.array()-position.array();
- velocity_error_.array() = desired_velocity.array()-velocity.array();
+ position_error_.array() = desired_position.array() - position.array();
+ velocity_error_.array() = desired_velocity.array() - velocity.array();
+
+ tau.array() = position_error_.array() * Kp.array() +
+ velocity_error_.array() * Kd.array();
- tau.array() = position_error_.array()*Kp.array()
- + velocity_error_.array()*Kd.array();
-
sotDEBUGOUT(15);
return tau;
-
}
-dynamicgraph::Vector& ControlPD::
-getPositionError( dynamicgraph::Vector &position_error, int t)
-{
- //sotDEBUGOUT(15) ??
+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 2fcb2018..eff881b5 100644
--- a/src/debug/contiifstream.cpp
+++ b/src/debug/contiifstream.cpp
@@ -12,52 +12,42 @@
using namespace dynamicgraph::sot;
-Contiifstream::
-Contiifstream( const std::string& n )
- :filename(n),cursor(0),first(true) {}
+Contiifstream::Contiifstream(const std::string &n)
+ : filename(n), cursor(0), first(true) {}
+Contiifstream::~Contiifstream(void) { sotDEBUGINOUT(5); }
-Contiifstream::
-~Contiifstream( void )
-{
- sotDEBUGINOUT(5);
-}
-
-
-bool Contiifstream::
-loop( void )
-{
+bool Contiifstream::loop(void) {
sotDEBUGIN(25);
- bool res=false;
+ bool res = false;
- std::fstream file( filename.c_str() );
+ std::fstream file(filename.c_str());
file.seekg(cursor);
file.sync();
- while(1)
- {
- file.get(buffer,BUFFER_SIZE);
- if( file.gcount() )
- {
- res=true;
- std::string line(buffer);
- if(! first) reader.push_back(line);
- cursor=file.tellg(); cursor++;
- file.get(*buffer); // get the last char ( = '\n')
- sotDEBUG(15) << "line: "<< line<<std::endl;
- }
- else { break; }
+ while (1) {
+ file.get(buffer, BUFFER_SIZE);
+ if (file.gcount()) {
+ res = true;
+ std::string line(buffer);
+ if (!first)
+ reader.push_back(line);
+ cursor = file.tellg();
+ cursor++;
+ file.get(*buffer); // get the last char ( = '\n')
+ sotDEBUG(15) << "line: " << line << std::endl;
+ } else {
+ break;
}
+ }
- first=false;
+ first = false;
sotDEBUGOUT(25);
return res;
}
-std::string
-Contiifstream::next( void )
-{
+std::string Contiifstream::next(void) {
std::string res = *reader.begin();
reader.pop_front();
return res;
diff --git a/src/debug/debug.cpp b/src/debug/debug.cpp
index c50ffaad..22f3486b 100644
--- a/src/debug/debug.cpp
+++ b/src/debug/debug.cpp
@@ -7,63 +7,59 @@
*
*/
-#include <sot/core/debug.hh>
#include <fstream>
#include <ios>
+#include <sot/core/debug.hh>
using namespace dynamicgraph::sot;
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
#ifdef WIN32
-const char * DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt";
-#else // WIN32
-const char * DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt";
-#endif // WIN32
+const char *DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt";
+#else // WIN32
+const char *DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt";
+#endif // WIN32
#ifdef VP_DEBUG
-# ifdef WIN32
- std::ofstream debugfile("C:/tmp/sot-core-traces.txt",
- std::ios::trunc&std::ios::out);
-# else // WIN32
- std::ofstream debugfile("/tmp/sot-core-traces.txt",
- std::ios::trunc&std::ios::out);
-# endif // WIN32
-#else // VP_DEBUG
+#ifdef WIN32
+std::ofstream debugfile("C:/tmp/sot-core-traces.txt",
+ std::ios::trunc &std::ios::out);
+#else // WIN32
+std::ofstream debugfile("/tmp/sot-core-traces.txt",
+ std::ios::trunc &std::ios::out);
+#endif // WIN32
+#else // VP_DEBUG
- std::ofstream debugfile;
+std::ofstream debugfile;
- class __sotDebug_init
- {
- public:
- __sotDebug_init()
- {
- debugfile.setstate (std::ios::failbit);
- }
- };
- __sotDebug_init __sotDebug_initialisator;
+class __sotDebug_init {
+public:
+ __sotDebug_init() { debugfile.setstate(std::ios::failbit); }
+};
+__sotDebug_init __sotDebug_initialisator;
#endif // VP_DEBUG
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
-void DebugTrace::openFile(const char * filename)
-{
- if (debugfile.good () && debugfile.is_open ())
- debugfile.close ();
- debugfile.clear ();
- debugfile.open (filename, std::ios::trunc&std::ios::out);
+void DebugTrace::openFile(const char *filename) {
+ 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 ())
+void DebugTrace::closeFile(const char *) {
+ if (debugfile.good() && debugfile.is_open())
debugfile.close();
- debugfile.setstate (std::ios::failbit);
+ debugfile.setstate(std::ios::failbit);
}
namespace dynamicgraph {
- namespace sot {
- DebugTrace sotDEBUGFLOW(debugfile);
- DebugTrace sotERRORFLOW(debugfile);
- } // namespace sot.
+namespace sot {
+DebugTrace sotDEBUGFLOW(debugfile);
+DebugTrace sotERRORFLOW(debugfile);
+} // namespace sot.
} // namespace dynamicgraph
diff --git a/src/exception/exception-abstract.cpp b/src/exception/exception-abstract.cpp
index 4dddc9d5..97d7ddd1 100644
--- a/src/exception/exception-abstract.cpp
+++ b/src/exception/exception-abstract.cpp
@@ -7,8 +7,8 @@
*
*/
-#include <sot/core/exception-abstract.hh>
#include <sot/core/debug.hh>
+#include <sot/core/exception-abstract.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -19,72 +19,51 @@ using namespace dynamicgraph::sot;
const std::string ExceptionAbstract::EXCEPTION_NAME = "Abstract";
-
-ExceptionAbstract::
-ExceptionAbstract (const int& _code,
- const string & _msg)
- :
- code (_code),
- message (_msg)
+ExceptionAbstract::ExceptionAbstract(const int &_code, const string &_msg)
+ : code(_code), message(_msg)
{
- return ;
-
+ return;
}
/* ------------------------------------------------------------------------ */
/* --- ACCESSORS ---------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
-const char *ExceptionAbstract::
-getMessage (void)
-{
- return (this->message) .c_str();
+const char *ExceptionAbstract::getMessage(void) {
+ return (this->message).c_str();
}
-const string &ExceptionAbstract::
-getStringMessage (void)
-{
- return this->message;
-}
-
-int ExceptionAbstract::
-getCode (void)
-{
- return this->code;
+const string &ExceptionAbstract::getStringMessage(void) {
+ return this->message;
}
-const char* ExceptionAbstract::what() const throw() {
- return message.c_str();
-}
+int ExceptionAbstract::getCode(void) { return this->code; }
+const char *ExceptionAbstract::what() const throw() { return message.c_str(); }
/* ------------------------------------------------------------------------- */
/* --- MODIFIORS ----------------------------------------------------------- */
/* ------------------------------------------------------------------------- */
#ifdef SOT_EXCEPTION_PASSING_PARAM
-
-ExceptionAbstract::Param& ExceptionAbstract::Param::
-initCopy( const Param& p )
-{
- sotDEBUGIN(25);
- if( p.pointersSet )
- {
- strncpy( function,p.functionPTR,BUFFER_SIZE);
- strncpy( file,p.filePTR,BUFFER_SIZE);
- line = p.line;
- pointersSet=false;
- set=true;
- } else set=false;
- sotDEBUGOUT(25);
- return *this;
+ExceptionAbstract::Param &ExceptionAbstract::Param::initCopy(const Param &p) {
+ sotDEBUGIN(25);
+ if (p.pointersSet) {
+ strncpy(function, p.functionPTR, BUFFER_SIZE);
+ strncpy(file, p.filePTR, BUFFER_SIZE);
+ line = p.line;
+ pointersSet = false;
+ set = true;
+ } else
+ set = false;
+ sotDEBUGOUT(25);
+ return *this;
}
-ExceptionAbstract::Param::
-Param( const int& _line, const char * _function, const char * _file )
- : functionPTR(_function),line(_line),filePTR(_file),pointersSet(true)
-{
- sotDEBUGINOUT(25);
+ExceptionAbstract::Param::Param(const int &_line, const char *_function,
+ const char *_file)
+ : functionPTR(_function), line(_line), filePTR(_file), pointersSet(true) {
+ sotDEBUGINOUT(25);
}
#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
@@ -92,28 +71,22 @@ Param( const int& _line, const char * _function, const char * _file )
/* --- OP << --------------------------------------------------------------- */
/* ------------------------------------------------------------------------- */
-
namespace dynamicgraph {
- namespace sot {
- ostream &
- operator << (ostream & os,
- const ExceptionAbstract & error) {
- os << error.getExceptionName()<<"Error [#" << error.code << "]: "
- << error.message << endl;
+namespace sot {
+ostream &operator<<(ostream &os, const ExceptionAbstract &error) {
+ os << error.getExceptionName() << "Error [#" << error.code
+ << "]: " << error.message << endl;
#ifdef SOT_EXCEPTION_PASSING_PARAM
- if( error.p.set )
- os << "Thrown from "<<error.p.file << ": "<<error.p.function
- <<" (#"<<error.p.line << ")"<<endl;
+ if (error.p.set)
+ os << "Thrown from " << error.p.file << ": " << error.p.function << " (#"
+ << error.p.line << ")" << endl;
#endif //#ifdef SOT_EXCEPTION_PASSING_PARAM
- return os;
- }
- } // namespace sot
+ return os;
+}
+} // namespace sot
} // namespace dynamicgraph
-
-
-
/** \file $Source$
*/
diff --git a/src/exception/exception-dynamic.cpp b/src/exception/exception-dynamic.cpp
index c5eb2d4d..e154d138 100644
--- a/src/exception/exception-dynamic.cpp
+++ b/src/exception/exception-dynamic.cpp
@@ -7,9 +7,9 @@
*
*/
+#include <cstdio>
#include <sot/core/exception-dynamic.hh>
#include <stdarg.h>
-#include <cstdio>
using namespace dynamicgraph::sot;
@@ -19,32 +19,26 @@ using namespace dynamicgraph::sot;
const std::string ExceptionDynamic::EXCEPTION_NAME = "Dynamic";
-ExceptionDynamic::
-ExceptionDynamic ( const ExceptionDynamic::ErrorCodeEnum& errcode,
- const std::string & msg )
- :ExceptionAbstract(errcode,msg)
-{
-}
+ExceptionDynamic::ExceptionDynamic(
+ const ExceptionDynamic::ErrorCodeEnum &errcode, const std::string &msg)
+ : ExceptionAbstract(errcode, msg) {}
-ExceptionDynamic::
-ExceptionDynamic ( const ExceptionDynamic::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... )
- :ExceptionAbstract(errcode,msg)
-{
+ExceptionDynamic::ExceptionDynamic(
+ const ExceptionDynamic::ErrorCodeEnum &errcode, const std::string &msg,
+ const char *format, ...)
+ : ExceptionAbstract(errcode, msg) {
va_list args;
- va_start(args,format);
+ va_start(args, format);
const unsigned int SIZE = 256;
- char buffer[SIZE];
- vsnprintf(buffer,SIZE,format,args);
+ char buffer[SIZE];
+ vsnprintf(buffer, SIZE, format, args);
message += buffer;
va_end(args);
}
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/exception/exception-factory.cpp b/src/exception/exception-factory.cpp
index e61fe41a..bda77b1e 100644
--- a/src/exception/exception-factory.cpp
+++ b/src/exception/exception-factory.cpp
@@ -7,10 +7,10 @@
*
*/
-#include <sot/core/exception-factory.hh>
+#include <cstdio>
#include <sot/core/debug.hh>
+#include <sot/core/exception-factory.hh>
#include <stdarg.h>
-#include <cstdio>
using namespace dynamicgraph::sot;
@@ -20,40 +20,37 @@ using namespace dynamicgraph::sot;
const std::string ExceptionFactory::EXCEPTION_NAME = "Factory";
-ExceptionFactory::
-ExceptionFactory ( const ExceptionFactory::ErrorCodeEnum& errcode,
- const std::string & msg )
- :ExceptionAbstract(errcode,msg)
-{
- sotDEBUGF( 15,"Created with message <%s>.",msg.c_str());
- sotDEBUG( 1) <<"Created with message <%s>."<<msg<<std::endl;
+ExceptionFactory::ExceptionFactory(
+ const ExceptionFactory::ErrorCodeEnum &errcode, const std::string &msg)
+ : ExceptionAbstract(errcode, msg) {
+ sotDEBUGF(15, "Created with message <%s>.", msg.c_str());
+ sotDEBUG(1) << "Created with message <%s>." << msg << std::endl;
}
-ExceptionFactory::
-ExceptionFactory ( const ExceptionFactory::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... )
- :ExceptionAbstract(errcode,msg)
-{
+ExceptionFactory::ExceptionFactory(
+ const ExceptionFactory::ErrorCodeEnum &errcode, const std::string &msg,
+ const char *format, ...)
+ : ExceptionAbstract(errcode, msg) {
va_list args;
- va_start(args,format);
+ va_start(args, format);
const unsigned int SIZE = 256;
- char buffer[SIZE];
- vsnprintf(buffer,SIZE,format,args);
+ char buffer[SIZE];
+ vsnprintf(buffer, SIZE, format, args);
- sotDEBUG(15) <<"Created "<<" with message <"
- <<msg<<"> and buffer <"<<buffer<<">. "<<std::endl;
+ sotDEBUG(15) << "Created "
+ << " with message <" << msg << "> and buffer <" << buffer
+ << ">. " << std::endl;
message += buffer;
va_end(args);
- sotDEBUG(1) << "Throw exception " << EXCEPTION_NAME << "[#" << errcode<<"]: "
- <<"<"<< message << ">."<<std::endl;
-
+ sotDEBUG(1) << "Throw exception " << EXCEPTION_NAME << "[#" << errcode
+ << "]: "
+ << "<" << message << ">." << std::endl;
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/exception/exception-feature.cpp b/src/exception/exception-feature.cpp
index 7f124101..34efa1ed 100644
--- a/src/exception/exception-feature.cpp
+++ b/src/exception/exception-feature.cpp
@@ -7,9 +7,9 @@
*
*/
+#include <cstdio>
#include <sot/core/exception-feature.hh>
#include <stdarg.h>
-#include <cstdio>
using namespace dynamicgraph::sot;
@@ -19,31 +19,26 @@ using namespace dynamicgraph::sot;
const std::string ExceptionFeature::EXCEPTION_NAME = "Feature";
-ExceptionFeature::
-ExceptionFeature ( const ExceptionFeature::ErrorCodeEnum& errcode,
- const std::string & msg )
- :ExceptionAbstract(errcode,msg)
-{
-}
+ExceptionFeature::ExceptionFeature(
+ const ExceptionFeature::ErrorCodeEnum &errcode, const std::string &msg)
+ : ExceptionAbstract(errcode, msg) {}
-ExceptionFeature::
-ExceptionFeature ( const ExceptionFeature::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... )
- :ExceptionAbstract(errcode,msg)
-{
+ExceptionFeature::ExceptionFeature(
+ const ExceptionFeature::ErrorCodeEnum &errcode, const std::string &msg,
+ const char *format, ...)
+ : ExceptionAbstract(errcode, msg) {
va_list args;
- va_start(args,format);
+ va_start(args, format);
const unsigned int SIZE = 256;
- char buffer[SIZE];
- vsnprintf(buffer,SIZE,format,args);
+ char buffer[SIZE];
+ vsnprintf(buffer, SIZE, format, args);
message = buffer;
va_end(args);
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/exception/exception-signal.cpp b/src/exception/exception-signal.cpp
index b23d63df..1cb6ce0b 100644
--- a/src/exception/exception-signal.cpp
+++ b/src/exception/exception-signal.cpp
@@ -7,9 +7,9 @@
*
*/
+#include <cstdio>
#include <sot/core/exception-signal.hh>
#include <stdarg.h>
-#include <cstdio>
using namespace dynamicgraph::sot;
@@ -19,32 +19,26 @@ using namespace dynamicgraph::sot;
const std::string ExceptionSignal::EXCEPTION_NAME = "Signal";
-ExceptionSignal::
-ExceptionSignal ( const ExceptionSignal::ErrorCodeEnum& errcode,
- const std::string & msg )
- :ExceptionAbstract(errcode,msg)
-{
-}
+ExceptionSignal::ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode,
+ const std::string &msg)
+ : ExceptionAbstract(errcode, msg) {}
-ExceptionSignal::
-ExceptionSignal ( const ExceptionSignal::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... )
- :ExceptionAbstract(errcode,msg)
-{
+ExceptionSignal::ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format,
+ ...)
+ : ExceptionAbstract(errcode, msg) {
va_list args;
- va_start(args,format);
+ va_start(args, format);
const unsigned int SIZE = 256;
- char buffer[SIZE];
- vsnprintf(buffer,SIZE,format,args);
+ char buffer[SIZE];
+ vsnprintf(buffer, SIZE, format, args);
message += buffer;
va_end(args);
}
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/exception/exception-task.cpp b/src/exception/exception-task.cpp
index 15bd1927..1d301291 100644
--- a/src/exception/exception-task.cpp
+++ b/src/exception/exception-task.cpp
@@ -7,10 +7,9 @@
*
*/
+#include <cstdio>
#include <sot/core/exception-task.hh>
#include <stdarg.h>
-#include <cstdio>
-
using namespace dynamicgraph::sot;
@@ -20,31 +19,25 @@ using namespace dynamicgraph::sot;
const std::string ExceptionTask::EXCEPTION_NAME = "Task";
-ExceptionTask::
-ExceptionTask ( const ExceptionTask::ErrorCodeEnum& errcode,
- const std::string & msg )
- :ExceptionAbstract(errcode,msg)
-{
-}
+ExceptionTask::ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode,
+ const std::string &msg)
+ : ExceptionAbstract(errcode, msg) {}
-ExceptionTask::
-ExceptionTask ( const ExceptionTask::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... )
- :ExceptionAbstract(errcode,msg)
-{
+ExceptionTask::ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...)
+ : ExceptionAbstract(errcode, msg) {
va_list args;
- va_start(args,format);
+ va_start(args, format);
const unsigned int SIZE = 256;
- char buffer[SIZE];
- vsnprintf(buffer,SIZE,format,args);
+ char buffer[SIZE];
+ vsnprintf(buffer, SIZE, format, args);
message = buffer;
va_end(args);
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/exception/exception-tools.cpp b/src/exception/exception-tools.cpp
index 6ea35649..6ef436bd 100644
--- a/src/exception/exception-tools.cpp
+++ b/src/exception/exception-tools.cpp
@@ -7,9 +7,9 @@
*
*/
+#include <cstdio>
#include <sot/core/exception-tools.hh>
#include <stdarg.h>
-#include <cstdio>
using namespace dynamicgraph::sot;
@@ -19,32 +19,25 @@ using namespace dynamicgraph::sot;
const std::string ExceptionTools::EXCEPTION_NAME = "Tools";
-ExceptionTools::
-ExceptionTools ( const ExceptionTools::ErrorCodeEnum& errcode,
- const std::string & msg )
- :ExceptionAbstract(errcode,msg)
-{
-}
+ExceptionTools::ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode,
+ const std::string &msg)
+ : ExceptionAbstract(errcode, msg) {}
-ExceptionTools::
-ExceptionTools ( const ExceptionTools::ErrorCodeEnum& errcode,
- const std::string & msg,const char* format, ... )
- :ExceptionAbstract(errcode,msg)
-{
+ExceptionTools::ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode,
+ const std::string &msg, const char *format, ...)
+ : ExceptionAbstract(errcode, msg) {
va_list args;
- va_start(args,format);
+ va_start(args, format);
const unsigned int SIZE = 256;
- char buffer[SIZE];
- vsnprintf(buffer,SIZE,format,args);
+ char buffer[SIZE];
+ vsnprintf(buffer, SIZE, format, args);
message += buffer;
va_end(args);
}
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/factory/additional-functions.cpp b/src/factory/additional-functions.cpp
index b23bea6a..a8a28e39 100644
--- a/src/factory/additional-functions.cpp
+++ b/src/factory/additional-functions.cpp
@@ -7,10 +7,10 @@
*
*/
+#include <dynamic-graph/signal.h>
+#include <sot/core/additional-functions.hh>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
-#include <sot/core/additional-functions.hh>
-#include <dynamic-graph/signal.h>
#include <sot/core/flags.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -19,38 +19,37 @@ using namespace dynamicgraph;
/* \brief Constructor. At creation, overloads (deregisters-then registers
* again) the 'new' function in the g_shell
*/
-AdditionalFunctions::AdditionalFunctions() {
-}
+AdditionalFunctions::AdditionalFunctions() {}
-AdditionalFunctions::~AdditionalFunctions() {
-}
+AdditionalFunctions::~AdditionalFunctions() {}
-void AdditionalFunctions::
-cmdFlagSet( const std::string& cmdLine, istringstream& cmdArg, std::ostream& os )
-{
- if( cmdLine == "help" )
- {
- os << " - set <obj1.sig1(flag type)> {#&|}START:END"
- << "\t\tSet or reset the flag value." <<endl;
- return;
- }
+void AdditionalFunctions::cmdFlagSet(const std::string &cmdLine,
+ istringstream &cmdArg, std::ostream &os) {
+ if (cmdLine == "help") {
+ os << " - set <obj1.sig1(flag type)> {#&|}START:END"
+ << "\t\tSet or reset the flag value." << endl;
+ return;
+ }
try {
- Signal<Flags,int> &sig1
- = dynamic_cast< Signal<Flags,int>& >
- (PoolStorage::getInstance()->getSignal(cmdArg));
-
- dgDEBUG(25) << "set..."<<endl;
- Flags fl; try { fl = sig1.accessCopy(); } catch(...) {}
+ Signal<Flags, int> &sig1 = dynamic_cast<Signal<Flags, int> &>(
+ PoolStorage::getInstance()->getSignal(cmdArg));
+
+ dgDEBUG(25) << "set..." << endl;
+ Flags fl;
+ try {
+ fl = sig1.accessCopy();
+ } catch (...) {
+ }
cmdArg >> std::ws >> fl;
- dgDEBUG(15) << "Fl=" << fl <<std::endl;
+ dgDEBUG(15) << "Fl=" << fl << std::endl;
sig1 = fl;
- } catch( ExceptionAbstract & err ) { throw; }
- catch( ... ) {
- DG_THROW ExceptionFactory( ExceptionFactory::SYNTAX_ERROR,
- "setflag: sig should be of flag type. ",
- "(while calling setflag).");
+ } catch (ExceptionAbstract &err) {
+ throw;
+ } catch (...) {
+ DG_THROW ExceptionFactory(ExceptionFactory::SYNTAX_ERROR,
+ "setflag: sig should be of flag type. ",
+ "(while calling setflag).");
}
-
}
diff --git a/src/factory/pool.cpp b/src/factory/pool.cpp
index 2144e73d..5a820bc9 100644
--- a/src/factory/pool.cpp
+++ b/src/factory/pool.cpp
@@ -16,176 +16,146 @@
#endif /*WIN32*/
/* --- SOT --- */
-#include <sot/core/pool.hh>
-#include <sot/core/debug.hh>
#include <dynamic-graph/entity.h>
-#include <sot/core/task-abstract.hh>
+#include <sot/core/debug.hh>
#include <sot/core/feature-abstract.hh>
+#include <sot/core/pool.hh>
+#include <sot/core/task-abstract.hh>
namespace dynamicgraph {
- namespace sot {
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- PoolStorage::
- ~PoolStorage( void )
- {
- sotDEBUGIN(15);
-
-
- sotDEBUGOUT(15);
- return;
- }
-
-
- /* --------------------------------------------------------------------- */
- void PoolStorage::
- registerTask( const std::string& entname,TaskAbstract* ent )
- {
- Tasks::iterator entkey = task.find(entname);
- if( entkey != task.end() ) // key does exist
- {
- throw ExceptionFactory(ExceptionFactory::OBJECT_CONFLICT,
- "Another task already defined with the "
- "same name. ",
- "Task name is <%s>.",entname.c_str() );
- }
- else
- {
- sotDEBUG(10) << "Register task <"<< entname
- << "> in the pool." <<std::endl;
- task[entname] = ent;
- }
- }
-
- TaskAbstract& PoolStorage::
- getTask( const std::string& name )
- {
- Tasks::iterator entPtr = task .find( name );
- if( entPtr == task.end() )
- {
- SOT_THROW ExceptionFactory( ExceptionFactory::UNREFERED_OBJECT,
- "Unknown task."," (while calling <%s>)",
- name.c_str() );
- }
- return *entPtr->second;
- }
-
-
-
- /* --------------------------------------------------------------------- */
- void PoolStorage::
- registerFeature( const std::string& entname,FeatureAbstract* ent )
- {
- Features::iterator entkey = feature.find(entname);
- if( entkey != feature.end() ) // key does exist
- {
- throw ExceptionFactory( ExceptionFactory::OBJECT_CONFLICT,
- "Another feature already defined with the"
- " same name. ",
- "Feature name is <%s>.",entname.c_str() );
- }
- else
- {
- sotDEBUG(10) << "Register feature <"<< entname
- << "> in the pool." <<std::endl;
- feature[entname] = ent;
- }
- }
-
- FeatureAbstract& PoolStorage::
- getFeature( const std::string& name )
- {
- Features::iterator entPtr = feature .find( name );
- if( entPtr == feature.end() )
- {
- SOT_THROW ExceptionFactory(ExceptionFactory::UNREFERED_OBJECT,
- "Unknown feature."," (while calling <%s>)",
- name.c_str() );
- }
- return *entPtr->second;
- }
-
- void PoolStorage::
- writeGraph(const std::string &aFileName)
- {
- size_t IdxPointFound = aFileName.rfind(".");
- std::string tmp1 = aFileName.substr(0,IdxPointFound);
- size_t IdxSeparatorFound = aFileName.rfind("/");
- std::string GenericName;
- if (IdxSeparatorFound!=std::string::npos)
- GenericName = tmp1.substr(IdxSeparatorFound,tmp1.length());
- else
- GenericName = tmp1;
-
- /* Reading local time */
- time_t ltime;
- ltime = time(NULL);
- struct tm ltimeformatted;
+namespace sot {
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+PoolStorage::~PoolStorage(void) {
+ sotDEBUGIN(15);
+
+ sotDEBUGOUT(15);
+ return;
+}
+
+/* --------------------------------------------------------------------- */
+void PoolStorage::registerTask(const std::string &entname, TaskAbstract *ent) {
+ Tasks::iterator entkey = task.find(entname);
+ if (entkey != task.end()) // key does exist
+ {
+ throw ExceptionFactory(ExceptionFactory::OBJECT_CONFLICT,
+ "Another task already defined with the "
+ "same name. ",
+ "Task name is <%s>.", entname.c_str());
+ } else {
+ sotDEBUG(10) << "Register task <" << entname << "> in the pool."
+ << std::endl;
+ task[entname] = ent;
+ }
+}
+
+TaskAbstract &PoolStorage::getTask(const std::string &name) {
+ Tasks::iterator entPtr = task.find(name);
+ if (entPtr == task.end()) {
+ SOT_THROW ExceptionFactory(ExceptionFactory::UNREFERED_OBJECT,
+ "Unknown task.", " (while calling <%s>)",
+ name.c_str());
+ }
+ return *entPtr->second;
+}
+
+/* --------------------------------------------------------------------- */
+void PoolStorage::registerFeature(const std::string &entname,
+ FeatureAbstract *ent) {
+ Features::iterator entkey = feature.find(entname);
+ if (entkey != feature.end()) // key does exist
+ {
+ throw ExceptionFactory(ExceptionFactory::OBJECT_CONFLICT,
+ "Another feature already defined with the"
+ " same name. ",
+ "Feature name is <%s>.", entname.c_str());
+ } else {
+ sotDEBUG(10) << "Register feature <" << entname << "> in the pool."
+ << std::endl;
+ feature[entname] = ent;
+ }
+}
+
+FeatureAbstract &PoolStorage::getFeature(const std::string &name) {
+ Features::iterator entPtr = feature.find(name);
+ if (entPtr == feature.end()) {
+ SOT_THROW ExceptionFactory(ExceptionFactory::UNREFERED_OBJECT,
+ "Unknown feature.", " (while calling <%s>)",
+ name.c_str());
+ }
+ return *entPtr->second;
+}
+
+void PoolStorage::writeGraph(const std::string &aFileName) {
+ size_t IdxPointFound = aFileName.rfind(".");
+ std::string tmp1 = aFileName.substr(0, IdxPointFound);
+ size_t IdxSeparatorFound = aFileName.rfind("/");
+ std::string GenericName;
+ if (IdxSeparatorFound != std::string::npos)
+ GenericName = tmp1.substr(IdxSeparatorFound, tmp1.length());
+ else
+ GenericName = tmp1;
+
+ /* Reading local time */
+ time_t ltime;
+ ltime = time(NULL);
+ struct tm ltimeformatted;
#ifdef WIN32
- localtime_s(<imeformatted,<ime);
+ localtime_s(<imeformatted, <ime);
#else
- localtime_r(<ime,<imeformatted);
+ localtime_r(<ime, <imeformatted);
#endif /*WIN32*/
- /* Opening the file and writing the first comment. */
- std::ofstream GraphFile;
- GraphFile.open(aFileName.c_str(),std::ofstream::out);
- GraphFile << "/* This graph has been automatically generated. "
- << std::endl;
- GraphFile << " " << 1900+ltimeformatted.tm_year
- << " Month: " << 1+ltimeformatted.tm_mon
- << " Day: " << ltimeformatted.tm_mday
- << " Time: " << ltimeformatted.tm_hour
- << ":" << ltimeformatted.tm_min;
- GraphFile << " */" << std::endl;
- GraphFile << "digraph " << GenericName << " { ";
- GraphFile << "\t graph [ label=\"" << GenericName
- << "\" bgcolor = white rankdir=LR ]" << std::endl
- << "\t node [ fontcolor = black, color = black, "
- "fillcolor = gold1, style=filled, shape=box ] ; " << std::endl;
- GraphFile << "\tsubgraph cluster_Tasks { " << std::endl;
- GraphFile << "\t\t color=blue; label=\"Tasks\";" << std::endl;
-
- for( Tasks::iterator iter=task.begin();
- iter!=task.end();iter++ )
- {
- TaskAbstract* ent = iter->second;
- GraphFile << "\t\t" << ent->getName()
- <<" [ label = \"" << ent->getName() << "\" ," << std::endl
- <<"\t\t fontcolor = black, color = black, "
- "fillcolor = magenta, style=filled, shape=box ]" << std::endl;
- }
-
-
- GraphFile << "}"<< std::endl;
-
- GraphFile.close();
- }
-
- void PoolStorage::
- writeCompletionList(std::ostream& /*os*/ )
- {
- }
-
- PoolStorage* PoolStorage::getInstance()
- {
- if (instance_ == 0) {
- instance_ = new PoolStorage;
- }
- return instance_;
- }
- void PoolStorage::destroy()
- {
- delete instance_;
- instance_ = 0;
- }
-
- PoolStorage::PoolStorage()
- {
- }
-
- PoolStorage* PoolStorage::instance_ = 0;
- } // namespace sot
+ /* Opening the file and writing the first comment. */
+ std::ofstream GraphFile;
+ GraphFile.open(aFileName.c_str(), std::ofstream::out);
+ GraphFile << "/* This graph has been automatically generated. " << std::endl;
+ GraphFile << " " << 1900 + ltimeformatted.tm_year
+ << " Month: " << 1 + ltimeformatted.tm_mon
+ << " Day: " << ltimeformatted.tm_mday
+ << " Time: " << ltimeformatted.tm_hour << ":"
+ << ltimeformatted.tm_min;
+ GraphFile << " */" << std::endl;
+ GraphFile << "digraph " << GenericName << " { ";
+ GraphFile << "\t graph [ label=\"" << GenericName
+ << "\" bgcolor = white rankdir=LR ]" << std::endl
+ << "\t node [ fontcolor = black, color = black, "
+ "fillcolor = gold1, style=filled, shape=box ] ; "
+ << std::endl;
+ GraphFile << "\tsubgraph cluster_Tasks { " << std::endl;
+ GraphFile << "\t\t color=blue; label=\"Tasks\";" << std::endl;
+
+ for (Tasks::iterator iter = task.begin(); iter != task.end(); iter++) {
+ TaskAbstract *ent = iter->second;
+ GraphFile << "\t\t" << ent->getName() << " [ label = \"" << ent->getName()
+ << "\" ," << std::endl
+ << "\t\t fontcolor = black, color = black, "
+ "fillcolor = magenta, style=filled, shape=box ]"
+ << std::endl;
+ }
+
+ GraphFile << "}" << std::endl;
+
+ GraphFile.close();
+}
+
+void PoolStorage::writeCompletionList(std::ostream & /*os*/) {}
+
+PoolStorage *PoolStorage::getInstance() {
+ if (instance_ == 0) {
+ instance_ = new PoolStorage;
+ }
+ return instance_;
+}
+void PoolStorage::destroy() {
+ delete instance_;
+ instance_ = 0;
+}
+
+PoolStorage::PoolStorage() {}
+
+PoolStorage *PoolStorage::instance_ = 0;
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/feature/feature-1d.cpp b/src/feature/feature-1d.cpp
index 231b3566..fde0ec72 100644
--- a/src/feature/feature-1d.cpp
+++ b/src/feature/feature-1d.cpp
@@ -13,80 +13,72 @@
/* --- SOT --- */
#include <sot/core/debug.hh>
-#include <sot/core/feature-1d.hh>
#include <sot/core/exception-feature.hh>
+#include <sot/core/feature-1d.hh>
using namespace std;
#include <sot/core/factory.hh>
-
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Feature1D,"Feature1D");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Feature1D, "Feature1D");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-Feature1D::
-Feature1D( const string& pointName )
- : FeatureAbstract( pointName )
- ,errorSIN( NULL,"sotFeature1D("+name+")::input(vector)::errorIN" )
- ,jacobianSIN( NULL,"sotFeature1D("+name+")::input(matrix)::jacobianIN" )
-{
- jacobianSOUT.addDependency( jacobianSIN );
- errorSOUT.addDependency( errorSIN );
+Feature1D::Feature1D(const string &pointName)
+ : FeatureAbstract(pointName),
+ errorSIN(NULL, "sotFeature1D(" + name + ")::input(vector)::errorIN"),
+ jacobianSIN(NULL,
+ "sotFeature1D(" + name + ")::input(matrix)::jacobianIN") {
+ jacobianSOUT.addDependency(jacobianSIN);
+ errorSOUT.addDependency(errorSIN);
- signalRegistration( errorSIN<<jacobianSIN );
+ signalRegistration(errorSIN << jacobianSIN);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void Feature1D::addDependenciesFromReference( void ){}
-void Feature1D::removeDependenciesFromReference( void ){}
+void Feature1D::addDependenciesFromReference(void) {}
+void Feature1D::removeDependenciesFromReference(void) {}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned int& Feature1D::
-getDimension( unsigned int & dim, int /*time*/ )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &Feature1D::getDimension(unsigned int &dim, int /*time*/) {
+ sotDEBUG(25) << "# In {" << endl;
dim = 1;
- sotDEBUG(25)<<"# Out }"<<endl;
+ sotDEBUG(25) << "# Out }" << endl;
return dim;
}
-
-dynamicgraph::Vector& Feature1D::
-computeError( dynamicgraph::Vector& res,int time )
-{
- const dynamicgraph::Vector& err = errorSIN.access(time);
- res.resize(1); res(0)=err.dot(err)*.5;
+dynamicgraph::Vector &Feature1D::computeError(dynamicgraph::Vector &res,
+ int time) {
+ const dynamicgraph::Vector &err = errorSIN.access(time);
+ res.resize(1);
+ res(0) = err.dot(err) * .5;
return res;
-
}
-
-
-dynamicgraph::Matrix& Feature1D::
-computeJacobian( dynamicgraph::Matrix& res,int time )
-{
+dynamicgraph::Matrix &Feature1D::computeJacobian(dynamicgraph::Matrix &res,
+ int time) {
sotDEBUGIN(15);
- const dynamicgraph::Matrix& Jac = jacobianSIN.access(time);
- const dynamicgraph::Vector& err = errorSIN.access(time);
+ const dynamicgraph::Matrix &Jac = jacobianSIN.access(time);
+ const dynamicgraph::Vector &err = errorSIN.access(time);
- 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);
+ 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);
sotDEBUGOUT(15);
return res;
@@ -96,13 +88,13 @@ computeJacobian( dynamicgraph::Matrix& res,int time )
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void Feature1D::
-display( std::ostream& os ) const
-{
- os <<"1D <"<<name<<">: " <<std::endl;
+void Feature1D::display(std::ostream &os) const {
+ os << "1D <" << name << ">: " << std::endl;
- try{
- os << " error= "<< errorSIN.accessCopy() << endl
- << " J = "<< jacobianSIN.accessCopy() << endl;
- } catch(ExceptionAbstract e){ os<< " All SIN not set."; }
+ try {
+ os << " error= " << errorSIN.accessCopy() << endl
+ << " J = " << jacobianSIN.accessCopy() << endl;
+ } catch (ExceptionAbstract e) {
+ os << " All SIN not set.";
+ }
}
diff --git a/src/feature/feature-abstract.cpp b/src/feature/feature-abstract.cpp
index 4900f100..2eeb603c 100644
--- a/src/feature/feature-abstract.cpp
+++ b/src/feature/feature-abstract.cpp
@@ -9,119 +9,114 @@
#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;
-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" )
- ,errorSOUT( boost::bind(&FeatureAbstract::computeError,this,_1,_2),
- selectionSIN,
- "sotFeatureAbstract("+name+")::output(vector)::error" )
- ,errordotSOUT (boost::bind(&FeatureAbstract::computeErrorDot,this,_1,_2),
- selectionSIN << errordotSIN,
- "sotFeatureAbstract("+name+")::output(vector)::errordot" )
-
- ,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" )
-{
+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"),
+ errorSOUT(boost::bind(&FeatureAbstract::computeError, this, _1, _2),
+ selectionSIN,
+ "sotFeatureAbstract(" + name + ")::output(vector)::error"),
+ errordotSOUT(boost::bind(&FeatureAbstract::computeErrorDot, this, _1, _2),
+ selectionSIN << errordotSIN,
+ "sotFeatureAbstract(" + name + ")::output(vector)::errordot")
+
+ ,
+ 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") {
selectionSIN = true;
- signalRegistration( selectionSIN
- <<errorSOUT<<jacobianSOUT<<dimensionSOUT );
+ signalRegistration(selectionSIN << errorSOUT << jacobianSOUT
+ << dimensionSOUT);
featureRegistration();
initCommands();
}
-void FeatureAbstract::
-initCommands( void )
-{
+void FeatureAbstract::initCommands(void) {
using namespace command;
addCommand("setReference",
- new dynamicgraph::command::Setter<FeatureAbstract, std::string>
- (*this, &FeatureAbstract::setReferenceByName,
- "Give the name of the reference feature.\nInput: a string (feature name)."));
+ new dynamicgraph::command::Setter<FeatureAbstract, std::string>(
+ *this, &FeatureAbstract::setReferenceByName,
+ "Give the name of the reference feature.\nInput: a string "
+ "(feature name)."));
addCommand("getReference",
- new dynamicgraph::command::Getter<FeatureAbstract, std::string>
- (*this, &FeatureAbstract::getReferenceByName,
- "Get the name of the reference feature.\nOutput: a string (feature name)."));
+ new dynamicgraph::command::Getter<FeatureAbstract, std::string>(
+ *this, &FeatureAbstract::getReferenceByName,
+ "Get the name of the reference feature.\nOutput: a string "
+ "(feature name)."));
}
-void FeatureAbstract::
-featureRegistration( void )
-{
- PoolStorage::getInstance()->registerFeature(name,this);
+void FeatureAbstract::featureRegistration(void) {
+ PoolStorage::getInstance()->registerFeature(name, this);
}
-std::ostream& FeatureAbstract::
-writeGraph( std::ostream& os ) const
-{
+std::ostream &FeatureAbstract::writeGraph(std::ostream &os) const {
Entity::writeGraph(os);
- if( isReferenceSet() )
- {
- const FeatureAbstract *asotFA = getReferenceAbstract();
- os << "\t\"" << asotFA->getName() << "\" -> \"" << getName() << "\""
- << "[ color=darkseagreen4 ]" << std::endl;
- }
- else std::cout << "asotFAT : 0" << std::endl;
+ if (isReferenceSet()) {
+ const FeatureAbstract *asotFA = getReferenceAbstract();
+ os << "\t\"" << asotFA->getName() << "\" -> \"" << getName() << "\""
+ << "[ color=darkseagreen4 ]" << std::endl;
+ } else
+ std::cout << "asotFAT : 0" << std::endl;
return os;
}
-void FeatureAbstract::
-setReferenceByName( const std::string& name )
-{
- setReference( &dynamicgraph::sot::PoolStorage::getInstance()->getFeature(name));
+void FeatureAbstract::setReferenceByName(const std::string &name) {
+ setReference(
+ &dynamicgraph::sot::PoolStorage::getInstance()->getFeature(name));
}
-std::string FeatureAbstract::
-getReferenceByName() const
-{
- if( isReferenceSet() ) return getReferenceAbstract()->getName(); else return "none";
+std::string FeatureAbstract::getReferenceByName() const {
+ if (isReferenceSet())
+ return getReferenceAbstract()->getName();
+ else
+ 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);
+ const int &dim = dimensionSOUT(time);
unsigned int curr = 0;
- res.resize( dim );
+ res.resize(dim);
sotDEBUG(25) << "Dim = " << dim << std::endl;
- if( isReferenceSet () && getReferenceAbstract ()->errordotSIN.isPlugged ())
- {
- const dynamicgraph::Vector& errdotDes = getReferenceAbstract ()->errordotSIN(time);
- sotDEBUG(15) << "Err* = " << errdotDes;
- if( errdotDes.size()<dim )
- { SOT_THROW ExceptionFeature( ExceptionFeature::UNCOMPATIBLE_SIZE,
- "Error: dimension uncompatible with des->errorIN size."
- " (while considering feature <%s>).",getName().c_str() ); }
-
- for( int i=0;i<errdotDes.size();++i )
- if( fl(i) )
- res( curr++ ) = - errdotDes(i);
+ if (isReferenceSet() && getReferenceAbstract()->errordotSIN.isPlugged()) {
+ const dynamicgraph::Vector &errdotDes =
+ getReferenceAbstract()->errordotSIN(time);
+ sotDEBUG(15) << "Err* = " << errdotDes;
+ if (errdotDes.size() < dim) {
+ SOT_THROW ExceptionFeature(
+ ExceptionFeature::UNCOMPATIBLE_SIZE,
+ "Error: dimension uncompatible with des->errorIN size."
+ " (while considering feature <%s>).",
+ getName().c_str());
}
- else
- res.setZero();
+
+ for (int i = 0; i < errdotDes.size(); ++i)
+ if (fl(i))
+ res(curr++) = -errdotDes(i);
+ } else
+ res.setZero();
return res;
}
diff --git a/src/feature/feature-generic.cpp b/src/feature/feature-generic.cpp
index 5574e56e..94cb5fe2 100644
--- a/src/feature/feature-generic.cpp
+++ b/src/feature/feature-generic.cpp
@@ -13,132 +13,127 @@
/* --- SOT --- */
#include <sot/core/debug.hh>
-#include <sot/core/feature-generic.hh>
#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
+#include <sot/core/feature-generic.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric,"FeatureGeneric");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-
-FeatureGeneric::
-FeatureGeneric( const string& pointName )
- : FeatureAbstract( pointName )
- ,dimensionDefault(0)
- ,errorSIN( NULL,"sotFeatureGeneric("+name+")::input(vector)::errorIN" )
- ,jacobianSIN( NULL,"sotFeatureGeneric("+name+")::input(matrix)::jacobianIN" )
+FeatureGeneric::FeatureGeneric(const string &pointName)
+ : FeatureAbstract(pointName), dimensionDefault(0),
+ errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"),
+ jacobianSIN(NULL,
+ "sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN")
{
- jacobianSOUT.addDependency( jacobianSIN );
- errorSOUT.addDependency( errorSIN );
+ jacobianSOUT.addDependency(jacobianSIN);
+ errorSOUT.addDependency(errorSIN);
- signalRegistration( errorSIN<<jacobianSIN << errordotSIN << errordotSOUT);
+ signalRegistration(errorSIN << jacobianSIN << errordotSIN << errordotSOUT);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void FeatureGeneric::addDependenciesFromReference( void )
-{
- assert( SP::isReferenceSet() );
- errorSOUT.addDependency( getReference()->errorSIN );
- errordotSOUT.addDependency( getReference()->errordotSIN );
+void FeatureGeneric::addDependenciesFromReference(void) {
+ assert(SP::isReferenceSet());
+ errorSOUT.addDependency(getReference()->errorSIN);
+ errordotSOUT.addDependency(getReference()->errordotSIN);
}
-void FeatureGeneric::removeDependenciesFromReference( void )
-{
- assert( SP::isReferenceSet() );
- errorSOUT.removeDependency( getReference()->errorSIN );
- errordotSOUT.removeDependency( getReference()->errordotSIN );
+void FeatureGeneric::removeDependenciesFromReference(void) {
+ assert(SP::isReferenceSet());
+ errorSOUT.removeDependency(getReference()->errorSIN);
+ errordotSOUT.removeDependency(getReference()->errordotSIN);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned int& FeatureGeneric::
-getDimension( unsigned int & dim, int time )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &FeatureGeneric::getDimension(unsigned int &dim, int time) {
+ sotDEBUG(25) << "# In {" << endl;
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++;
+ for (unsigned int i = 0; i < dimensionDefault; ++i)
+ if (fl(i))
+ dim++;
- sotDEBUG(25)<<"# Out }"<<endl;
+ sotDEBUG(25) << "# Out }" << endl;
return dim;
}
-
-Vector& FeatureGeneric::
-computeError( Vector& res,int time )
-{
- const Vector& err = errorSIN.access(time);
+Vector &FeatureGeneric::computeError(Vector &res, int time) {
+ const Vector &err = errorSIN.access(time);
const Flags &fl = selectionSIN.access(time);
- const int & dim = dimensionSOUT(time);
+ const int &dim = dimensionSOUT(time);
unsigned int curr = 0;
- res.resize( dim );
- if( err.size()<dim )
- { SOT_THROW ExceptionFeature( ExceptionFeature::UNCOMPATIBLE_SIZE,
- "Error: dimension uncompatible with des->errorIN size."
- " (while considering feature <%s>).",getName().c_str() ); }
+ res.resize(dim);
+ if (err.size() < dim) {
+ SOT_THROW ExceptionFeature(
+ ExceptionFeature::UNCOMPATIBLE_SIZE,
+ "Error: dimension uncompatible with des->errorIN size."
+ " (while considering feature <%s>).",
+ getName().c_str());
+ }
sotDEBUG(15) << "Err = " << err;
sotDEBUG(25) << "Dim = " << dim << endl;
- if( isReferenceSet() )
- {
- const Vector& errDes = getReference()->errorSIN(time);
- sotDEBUG(15) << "Err* = " << errDes;
- if( errDes.size()<dim )
- { SOT_THROW ExceptionFeature( ExceptionFeature::UNCOMPATIBLE_SIZE,
- "Error: dimension uncompatible with des->errorIN size."
- " (while considering feature <%s>).",getName().c_str() ); }
-
- for( int i=0;i<err.size();++i ) if( fl(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 (isReferenceSet()) {
+ const Vector &errDes = getReference()->errorSIN(time);
+ sotDEBUG(15) << "Err* = " << errDes;
+ if (errDes.size() < dim) {
+ SOT_THROW ExceptionFeature(
+ ExceptionFeature::UNCOMPATIBLE_SIZE,
+ "Error: dimension uncompatible with des->errorIN size."
+ " (while considering feature <%s>).",
+ getName().c_str());
}
- return res;
+ for (int i = 0; i < err.size(); ++i)
+ if (fl(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);
+ }
+ return res;
}
-Matrix& FeatureGeneric::
-computeJacobian( Matrix& res,int time )
-{
+Matrix &FeatureGeneric::computeJacobian(Matrix &res, int time) {
sotDEBUGIN(15);
- const Matrix& Jac = jacobianSIN.access(time);
+ const Matrix &Jac = jacobianSIN.access(time);
const Flags &fl = selectionSIN.access(time);
const unsigned int &dim = dimensionSOUT(time);
unsigned int curr = 0;
- res.resize( dim,Jac.cols() );
+ res.resize(dim, Jac.cols());
- 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);
- curr++;
- }
+ 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);
+ curr++;
+ }
sotDEBUGOUT(15);
return res;
@@ -148,13 +143,13 @@ computeJacobian( Matrix& res,int time )
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void FeatureGeneric::
-display( std::ostream& os ) const
-{
- os <<"Generic <"<<name<<">: " <<std::endl;
+void FeatureGeneric::display(std::ostream &os) const {
+ os << "Generic <" << name << ">: " << std::endl;
- try{
- os << " error= "<< errorSIN.accessCopy() << endl
- << " J = "<< jacobianSIN.accessCopy() << endl;
- } catch(ExceptionSignal & e){ os << e.what(); }
+ try {
+ os << " error= " << errorSIN.accessCopy() << endl
+ << " J = " << jacobianSIN.accessCopy() << endl;
+ } catch (ExceptionSignal &e) {
+ os << e.what();
+ }
}
diff --git a/src/feature/feature-joint-limits-command.h b/src/feature/feature-joint-limits-command.h
index 21d4c367..f998e4db 100644
--- a/src/feature/feature-joint-limits-command.h
+++ b/src/feature/feature-joint-limits-command.h
@@ -7,43 +7,41 @@
*/
#ifndef FEATURE_JOINT_LIMITS_COMMAND_H
- #define FEATURE_JOINT_LIMITS_COMMAND_H
+#define FEATURE_JOINT_LIMITS_COMMAND_H
- #include <boost/assign/list_of.hpp>
+#include <boost/assign/list_of.hpp>
- #include <dynamic-graph/command.h>
- #include <dynamic-graph/command-setter.h>
- #include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command.h>
-namespace dynamicgraph { namespace sot {
- namespace command {
- namespace featureJointLimits {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
+namespace dynamicgraph {
+namespace sot {
+namespace command {
+namespace featureJointLimits {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
- // Command Actuate
- class Actuate : public Command
- {
- public:
- virtual ~Actuate() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Actuate(FeatureJointLimits& entity, const std::string& docstring) :
- Command(entity, std::vector<Value::Type>(), docstring)
- {
- }
- virtual Value doExecute()
- {
- FeatureJointLimits& fjl = static_cast<FeatureJointLimits&>(owner());
- Flags fl( 63 ); //0x0000003f = 00000000000000000000000000111111
- fjl.selectionSIN = (! fl);
- // return void
- return Value();
- }
- }; // class Actuate
- } // namespace featureJointLimits
- } // namespace command
-} /* namespace sot */} /* namespace dynamicgraph */
+// Command Actuate
+class Actuate : public Command {
+public:
+ virtual ~Actuate() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Actuate(FeatureJointLimits &entity, const std::string &docstring)
+ : Command(entity, std::vector<Value::Type>(), docstring) {}
+ virtual Value doExecute() {
+ FeatureJointLimits &fjl = static_cast<FeatureJointLimits &>(owner());
+ Flags fl(63); // 0x0000003f = 00000000000000000000000000111111
+ fjl.selectionSIN = (!fl);
+ // return void
+ return Value();
+ }
+}; // 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 96a35db7..be88135b 100644
--- a/src/feature/feature-joint-limits.cpp
+++ b/src/feature/feature-joint-limits.cpp
@@ -12,13 +12,13 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
-#include <sot/core/feature-joint-limits.hh>
-#include <sot/core/exception-feature.hh>
#include <sot/core/debug.hh>
+#include <sot/core/exception-feature.hh>
+#include <sot/core/feature-joint-limits.hh>
using namespace std;
-#include <sot/core/factory.hh>
#include <../src/feature/feature-joint-limits-command.h>
+#include <sot/core/factory.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
@@ -26,78 +26,76 @@ using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureJointLimits,"FeatureJointLimits");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureJointLimits, "FeatureJointLimits");
const double FeatureJointLimits::THRESHOLD_DEFAULT = .9;
-
-FeatureJointLimits::
-FeatureJointLimits( const string& fName )
- : FeatureAbstract( fName )
- ,threshold(THRESHOLD_DEFAULT)
-
- ,jointSIN( NULL,"sotFeatureJointLimits("+name+")::input(vector)::joint" )
- ,upperJlSIN( NULL,"sotFeatureJointLimits("+name+")::input(vector)::upperJl" )
- ,lowerJlSIN( NULL,"sotFeatureJointLimits("+name+")::input(vector)::lowerJl" )
- ,widthJlSINTERN( boost::bind(&FeatureJointLimits::computeWidthJl,this,_1,_2),
- upperJlSIN<<lowerJlSIN,
- "sotFeatureJointLimits("+name+")::input(vector)::widthJl" )
-{
- errorSOUT.addDependency( jointSIN );
- errorSOUT.addDependency( upperJlSIN );
- errorSOUT.addDependency( lowerJlSIN );
-
- signalRegistration( jointSIN<<upperJlSIN<<lowerJlSIN<<widthJlSINTERN );
+FeatureJointLimits::FeatureJointLimits(const string &fName)
+ : FeatureAbstract(fName), threshold(THRESHOLD_DEFAULT)
+
+ ,
+ jointSIN(NULL,
+ "sotFeatureJointLimits(" + name + ")::input(vector)::joint"),
+ upperJlSIN(NULL,
+ "sotFeatureJointLimits(" + name + ")::input(vector)::upperJl"),
+ lowerJlSIN(NULL,
+ "sotFeatureJointLimits(" + name + ")::input(vector)::lowerJl"),
+ widthJlSINTERN(
+ boost::bind(&FeatureJointLimits::computeWidthJl, this, _1, _2),
+ upperJlSIN << lowerJlSIN,
+ "sotFeatureJointLimits(" + name + ")::input(vector)::widthJl") {
+ errorSOUT.addDependency(jointSIN);
+ errorSOUT.addDependency(upperJlSIN);
+ errorSOUT.addDependency(lowerJlSIN);
+
+ signalRegistration(jointSIN << upperJlSIN << lowerJlSIN << widthJlSINTERN);
// Commands
//
std::string docstring;
// Actuate
docstring = " \n"
- " Actuate\n"
- " \n";
+ " Actuate\n"
+ " \n";
addCommand("actuate",
- new command::featureJointLimits::Actuate(*this, docstring));
+ new command::featureJointLimits::Actuate(*this, docstring));
}
/* --------------------------------------------------------------------- */
-void FeatureJointLimits::addDependenciesFromReference( void ){}
-void FeatureJointLimits::removeDependenciesFromReference( void ){}
-
+void FeatureJointLimits::addDependenciesFromReference(void) {}
+void FeatureJointLimits::removeDependenciesFromReference(void) {}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned int& FeatureJointLimits::
-getDimension( unsigned int & dim, int time )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &FeatureJointLimits::getDimension(unsigned int &dim, int time) {
+ sotDEBUG(25) << "# In {" << endl;
const Flags &fl = selectionSIN.access(time);
- const Matrix::Index NBJL = upperJlSIN.access(time).size();
+ const Matrix::Index NBJL = upperJlSIN.access(time).size();
dim = 0;
- for( Matrix::Index i=0;i<NBJL;++i )
- if( fl(static_cast<int>(i)) ) dim++;
+ for (Matrix::Index i = 0; i < NBJL; ++i)
+ if (fl(static_cast<int>(i)))
+ dim++;
- sotDEBUG(25)<<"# Out }"<<endl;
+ sotDEBUG(25) << "# Out }" << endl;
return dim;
}
-Vector&
-FeatureJointLimits::computeWidthJl( Vector& res,const int& time )
-{
+Vector &FeatureJointLimits::computeWidthJl(Vector &res, const int &time) {
sotDEBUGIN(15);
const Vector UJL = upperJlSIN.access(time);
const Vector LJL = lowerJlSIN.access(time);
- const Vector::Index SIZE=UJL.size();
+ const Vector::Index SIZE = UJL.size();
res.resize(SIZE);
- for( Vector::Index i=0;i<SIZE;++i )
- { res(i)=UJL(i)-LJL(i); }
+ for (Vector::Index i = 0; i < SIZE; ++i) {
+ res(i) = UJL(i) - LJL(i);
+ }
sotDEBUGOUT(15);
return res;
@@ -106,52 +104,49 @@ FeatureJointLimits::computeWidthJl( Vector& res,const int& time )
/** Compute the interaction matrix from a subset of
* the possible features.
*/
-Matrix& FeatureJointLimits::
-computeJacobian( Matrix& J,int time )
-{
- sotDEBUG(15)<<"# In {"<<endl;
+Matrix &FeatureJointLimits::computeJacobian(Matrix &J, int time) {
+ sotDEBUG(15) << "# In {" << endl;
- const unsigned int SIZE=dimensionSOUT.access(time);
+ const unsigned int SIZE = dimensionSOUT.access(time);
const Vector q = jointSIN.access(time);
const Flags &fl = selectionSIN(time);
- //const unsigned int SIZE_FF=SIZE+freeFloatingSize;
- const Vector::Index SIZE_TOTAL=q.size();
+ // const unsigned int SIZE_FF=SIZE+freeFloatingSize;
+ const Vector::Index SIZE_TOTAL = q.size();
const Vector WJL = widthJlSINTERN.access(time);
- J.resize( SIZE,SIZE_TOTAL ); J.setZero();
-
- unsigned int idx=0;
- for( unsigned int i=0;i<SIZE_TOTAL;++i )
- {
- if( fl(i) )
- {
- if( fabs(WJL(i))>1e-3 ) J(idx,i)=1/WJL(i);
- else J(idx,i)=1.;
- idx++;
- }
+ J.resize(SIZE, SIZE_TOTAL);
+ J.setZero();
+
+ unsigned int idx = 0;
+ for (unsigned int i = 0; i < SIZE_TOTAL; ++i) {
+ if (fl(i)) {
+ if (fabs(WJL(i)) > 1e-3)
+ J(idx, i) = 1 / WJL(i);
+ else
+ J(idx, i) = 1.;
+ idx++;
}
-// if( 0!=freeFloatingIndex )
-// for( unsigned int i=0;i<freeFloatingIndex;++i )
-// {
-// if( fabs(WJL(i))>1e-3 ) J(i,i)=1/WJL(i); else J(i,i)=1.;
-// }
-
-// if( SIZE!=freeFloatingIndex )
-// for( unsigned int i=freeFloatingIndex;i<SIZE;++i )
-// {
-// if( fabs(WJL(i))>1e-3 ) J(i,i+freeFloatingSIZE)=1/WJL(i);
-// else J(i,i)=1.;
-// }
-
- sotDEBUG(15)<<"# Out }"<<endl;
+ }
+ // if( 0!=freeFloatingIndex )
+ // for( unsigned int i=0;i<freeFloatingIndex;++i )
+ // {
+ // if( fabs(WJL(i))>1e-3 ) J(i,i)=1/WJL(i); else J(i,i)=1.;
+ // }
+
+ // if( SIZE!=freeFloatingIndex )
+ // for( unsigned int i=freeFloatingIndex;i<SIZE;++i )
+ // {
+ // if( fabs(WJL(i))>1e-3 ) J(i,i+freeFloatingSIZE)=1/WJL(i);
+ // else J(i,i)=1.;
+ // }
+
+ sotDEBUG(15) << "# Out }" << endl;
return J;
}
/** Compute the error between two visual features from a subset
* a the possible features.
*/
-Vector&
-FeatureJointLimits::computeError( Vector& error,int time )
-{
+Vector &FeatureJointLimits::computeError(Vector &error, int time) {
sotDEBUGIN(15);
const Flags &fl = selectionSIN(time);
@@ -159,35 +154,32 @@ FeatureJointLimits::computeError( Vector& error,int time )
const Vector UJL = upperJlSIN.access(time);
const Vector LJL = lowerJlSIN.access(time);
const Vector WJL = widthJlSINTERN.access(time);
- const int SIZE=dimensionSOUT.access(time);
- const Vector::Index SIZE_TOTAL=q.size();
+ const int SIZE = dimensionSOUT.access(time);
+ const Vector::Index SIZE_TOTAL = q.size();
sotDEBUG(25) << "q = " << q << endl;
sotDEBUG(25) << "ljl = " << LJL << endl;
sotDEBUG(25) << "Wjl = " << WJL << endl;
sotDEBUG(25) << "dim = " << SIZE << endl;
- assert( UJL.size() == SIZE_TOTAL );
- assert( WJL.size() == SIZE_TOTAL );
- assert( LJL.size() == SIZE_TOTAL );
- assert( SIZE <= SIZE_TOTAL );
+ assert(UJL.size() == SIZE_TOTAL);
+ assert(WJL.size() == SIZE_TOTAL);
+ assert(LJL.size() == SIZE_TOTAL);
+ assert(SIZE <= SIZE_TOTAL);
error.resize(SIZE);
unsigned int parcerr = 0;
- for( int i=0;i<SIZE_TOTAL;++i )
- {
- if( fl(i) )
- { error(parcerr++) = (q(i)-LJL(i))/WJL(i)*2-1; }
+ for (int i = 0; i < SIZE_TOTAL; ++i) {
+ if (fl(i)) {
+ error(parcerr++) = (q(i) - LJL(i)) / WJL(i) * 2 - 1;
}
+ }
sotDEBUGOUT(15);
- return error ;
+ return error;
}
-
-void FeatureJointLimits::
-display( std::ostream& os ) const
-{
- os <<"JointLimits <"<<name<<"> ... TODO";
+void FeatureJointLimits::display(std::ostream &os) const {
+ os << "JointLimits <" << name << "> ... TODO";
}
diff --git a/src/feature/feature-line-distance.cpp b/src/feature/feature-line-distance.cpp
index 90fc8d5a..36622bd6 100644
--- a/src/feature/feature-line-distance.cpp
+++ b/src/feature/feature-line-distance.cpp
@@ -13,8 +13,8 @@
/* --- SOT --- */
#include <sot/core/debug.hh>
-#include <sot/core/feature-line-distance.hh>
#include <sot/core/exception-feature.hh>
+#include <sot/core/feature-line-distance.hh>
#include <sot/core/matrix-geometry.hh>
@@ -24,202 +24,259 @@ using namespace dynamicgraph::sot;
using namespace dynamicgraph;
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureLineDistance,"FeatureLineDistance");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureLineDistance, "FeatureLineDistance");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-FeatureLineDistance::
-FeatureLineDistance( const string& pointName )
- : FeatureAbstract( pointName )
- ,positionSIN( NULL,"sotFeatureLineDistance("+name+")::input(matrixHomo)::position" )
- ,articularJacobianSIN( NULL,"sotFeatureLineDistance("+name+")::input(matrix)::Jq" )
- ,positionRefSIN( NULL,"sotFeatureLineDistance("+name+")::input(vector)::positionRef" )
- ,vectorSIN( NULL,"sotFeatureVector3("+name+")::input(vector3)::vector" )
- ,lineSOUT( boost::bind(&FeatureLineDistance::computeLineCoordinates,this,_1,_2),
- positionSIN<<positionRefSIN,
- "sotFeatureAbstract("+name+")::output(vector)::line" )
-{
- jacobianSOUT.addDependency( positionSIN );
- jacobianSOUT.addDependency( articularJacobianSIN );
-
- errorSOUT.addDependency( positionSIN );
-
- signalRegistration( positionSIN<<articularJacobianSIN
- <<positionRefSIN<<lineSOUT<<vectorSIN );
+FeatureLineDistance::FeatureLineDistance(const string &pointName)
+ : FeatureAbstract(pointName),
+ positionSIN(NULL, "sotFeatureLineDistance(" + name +
+ ")::input(matrixHomo)::position"),
+ articularJacobianSIN(NULL, "sotFeatureLineDistance(" + name +
+ ")::input(matrix)::Jq"),
+ positionRefSIN(NULL, "sotFeatureLineDistance(" + name +
+ ")::input(vector)::positionRef"),
+ vectorSIN(NULL,
+ "sotFeatureVector3(" + name + ")::input(vector3)::vector"),
+ lineSOUT(boost::bind(&FeatureLineDistance::computeLineCoordinates, this,
+ _1, _2),
+ positionSIN << positionRefSIN,
+ "sotFeatureAbstract(" + name + ")::output(vector)::line") {
+ jacobianSOUT.addDependency(positionSIN);
+ jacobianSOUT.addDependency(articularJacobianSIN);
+
+ errorSOUT.addDependency(positionSIN);
+
+ signalRegistration(positionSIN << articularJacobianSIN << positionRefSIN
+ << lineSOUT << vectorSIN);
}
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned int& FeatureLineDistance::
-getDimension( unsigned int & dim, int /*time*/ )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &FeatureLineDistance::getDimension(unsigned int &dim,
+ int /*time*/) {
+ sotDEBUG(25) << "# In {" << endl;
- return dim=1;
+ return dim = 1;
}
/* --------------------------------------------------------------------- */
-Vector& FeatureLineDistance::
-computeLineCoordinates( Vector& cood,int time )
-{
+Vector &FeatureLineDistance::computeLineCoordinates(Vector &cood, int time) {
sotDEBUGIN(15);
cood.resize(6);
/* Line coordinates */
const MatrixHomogeneous &pos = positionSIN(time);
- const Vector & vect = vectorSIN(time);
- MatrixRotation R; R = pos.linear();
- Vector v(3); v = R*vect;
-
- cood(0)= pos(0,3);
- cood(1)= pos(1,3);
- cood(2)= pos(2,3);
- cood(3)= v(0);
- cood(4)= v(1);
- cood(5)= v(2);
+ const Vector &vect = vectorSIN(time);
+ MatrixRotation R;
+ R = pos.linear();
+ Vector v(3);
+ v = R * vect;
+
+ cood(0) = pos(0, 3);
+ cood(1) = pos(1, 3);
+ cood(2) = pos(2, 3);
+ cood(3) = v(0);
+ cood(4) = v(1);
+ cood(5) = v(2);
sotDEBUGOUT(15);
return cood;
}
-
/* --------------------------------------------------------------------- */
/** Compute the interaction matrix from a subset of
* the possible features.
*/
-Matrix& FeatureLineDistance::
-computeJacobian( Matrix& J,int time )
-{
- sotDEBUG(15)<<"# In {"<<endl;
+Matrix &FeatureLineDistance::computeJacobian(Matrix &J, int time) {
+ sotDEBUG(15) << "# In {" << endl;
/* --- Compute the jacobian of the line coordinates --- */
Matrix Jline;
{
- const Matrix & Jq = articularJacobianSIN(time);
-
- const Vector & vect = vectorSIN(time);
- const MatrixHomogeneous & M = positionSIN(time);
- MatrixRotation R; R= M.linear(); // wRh
-
- Matrix Skew(3,3);
- Skew( 0,0 ) = 0 ; Skew( 0,1 )=-vect( 2 ); Skew( 0,2 ) = vect( 1 );
- Skew( 1,0 ) = vect( 2 ); Skew( 1,1 )= 0 ; Skew( 1,2 ) =-vect( 0 );
- Skew( 2,0 ) =-vect( 1 ); Skew( 2,1 )= vect( 0 ); Skew( 2,2 ) = 0 ;
-
- Matrix RSk(3,3); RSk = R*Skew;
-
- Jline.resize(6,Jq.cols());
- for( unsigned int i=0;i<3;++i )
- for( int j=0;j<Jq.cols();++j )
- {
- Jline(i,j) = 0;
- Jline(i+3,j) = 0;
- for( unsigned int k=0;k<3;++k )
- {
- Jline(i,j) += R(i,k)*Jq(k,j);
- Jline(i+3,j) += -RSk(i,k)*Jq(k+3,j);
- }
+ const Matrix &Jq = articularJacobianSIN(time);
+
+ const Vector &vect = vectorSIN(time);
+ const MatrixHomogeneous &M = positionSIN(time);
+ MatrixRotation R;
+ R = M.linear(); // wRh
+
+ Matrix Skew(3, 3);
+ Skew(0, 0) = 0;
+ Skew(0, 1) = -vect(2);
+ Skew(0, 2) = vect(1);
+ Skew(1, 0) = vect(2);
+ Skew(1, 1) = 0;
+ Skew(1, 2) = -vect(0);
+ Skew(2, 0) = -vect(1);
+ Skew(2, 1) = vect(0);
+ Skew(2, 2) = 0;
+
+ Matrix RSk(3, 3);
+ RSk = R * Skew;
+
+ Jline.resize(6, Jq.cols());
+ for (unsigned int i = 0; i < 3; ++i)
+ for (int j = 0; j < Jq.cols(); ++j) {
+ Jline(i, j) = 0;
+ Jline(i + 3, j) = 0;
+ for (unsigned int k = 0; k < 3; ++k) {
+ Jline(i, j) += R(i, k) * Jq(k, j);
+ Jline(i + 3, j) += -RSk(i, k) * Jq(k + 3, j);
}
+ }
}
/* --- Compute the jacobian wrt the line coordinates --- */
const Vector &line = lineSOUT(time);
- const double & x0 = line(0);
- const double & y0 = line(1);
- const double & z0 = line(2);
- const double & a0 = line(3);
- const double & b0 = line(4);
- const double & c0 = line(5);
+ const double &x0 = line(0);
+ const double &y0 = line(1);
+ const double &z0 = line(2);
+ const double &a0 = line(3);
+ const double &b0 = line(4);
+ const double &c0 = line(5);
const Vector &posRef = positionRefSIN(time);
- const double & x1 = posRef(0);
- const double & y1 = posRef(1);
- const double & z1 = posRef(2);
- const double & a1 = posRef(3);
- const double & b1 = posRef(4);
- const double & c1 = posRef(5);
+ const double &x1 = posRef(0);
+ const double &y1 = posRef(1);
+ const double &z1 = posRef(2);
+ const double &a1 = posRef(3);
+ const double &b1 = posRef(4);
+ const double &c1 = posRef(5);
/* Differential */
- const double a1_3 = a1*a1*a1;
- const double b1_3 = b1*b1*b1;
- const double c1_3 = c1*c1*c1;
-
- double K = c0*c0*a1*a1 - 2*c0*a1*a0*c1 - 2*c0*b1*b0*c1 + c0*c0*b1*b1 - 2*b0*a1*a0*b1 + b0*b0*a1*a1 + b0*b0*c1*c1 + a0*a0*b1*b1 + a0*a0*c1*c1;
-
- const double diffx0 = -b0*c1 + c0*b1;
- const double diffy0 = a0*c1 - c0*a1;
- const double diffz0 = -a0*b1 + b0*a1;
-
- const double diffa0 = 2*b0*c1*x0*a0*b1*b1 + 2*c0*b1*b1*x0*b0*a1 + 2*c0*c0*b1*x0*c1*a1 + 2*c1*c1*y0*c0*a1*a0 - 2*b0*c1*c1*x0*c0*a1 - 2*b0*b0*c1*x0*b1*a1 - 2*c1*c1*y0*c0*b1*b0 + 2*b0*b0*c1*x1*b1*a1 + 2*b0*c1*c1*x1*c0*a1 - 2*b0*c1*x1*a0*b1*b1 - c1*y0*c0*c0*a1*a1 + c1*y0*c0*c0*b1*b1 + c1*y0*b0*b0*a1*a1 - c1*y0*a0*a0*b1*b1 + c1*y1*c0*c0*a1*a1 - c1*y1*c0*c0*b1*b1 - c1*y1*b0*b0*a1*a1 + c1*y1*a0*a0*b1*b1 - b1*z0*c0*c0*a1*a1 + b1*z0*b0*b0*a1*a1 - b1*z0*b0*b0*c1*c1 + b1*z0*a0*a0*c1*c1 + b1*z1*c0*c0*a1*a1 - b1*z1*b0*b0*a1*a1 + b1*z1*b0*b0*c1*c1 - b1*z1*a0*a0*c1*c1 + 2*b0* c1_3*x0*a0 - 2*b0* c1_3*x1*a0 - 2*c0* b1_3*x0*a0 + 2*c0* b1_3*x1*a0 + c1_3*y0*b0*b0 - c1_3*y0*a0*a0 - c1_3*y1*b0*b0 + c1_3*y1*a0*a0 - b1_3*z0*c0*c0 + b1_3*z0*a0*a0 + b1_3*z1*c0*c0 - b1_3*z1*a0*a0 - 2*c1*c1*y1*c0*a1*a0 + 2*c1*c1*y1*c0*b1*b0 + 2*b1*b1*z0*c0*b0*c1 - 2*b1*b1*z0*b0*a1*a0 - 2*b1*b1*z1*c0*b0*c1 + 2*b1*b1*z1*b0*a1*a0 - 2*c0*b1*x0*a0*c1*c1 - 2*c0*b1*b1*x1*b0*a1 - 2*c0*c0*b1*x1*c1*a1 + 2*c0*b1*x1*a0*c1*c1 + 2*c0*a1*y0*a0*b1*b1 - 2*c0*a1*a1*y0*b1*b0 - 2*c0*a1*y1*a0*b1*b1 + 2*c0*a1*a1*y1*b1*b0 + 2*b0*a1*a1*z0*c1*c0 - 2*b0*a1*z0*a0*c1*c1 - 2*b0*a1*a1*z1*c1*c0 + 2*b0*a1*z1*a0*c1*c1;
- const double diffb0 = -2*c1*c1*x0*c0*b1*b0 + 2*c1*c1*x0*c0*a1*a0 - c1*x0*c0*c0*a1*a1 + c1*x0*c0*c0*b1*b1 + c1*x0*b0*b0*a1*a1 - c1*x0*a0*a0*b1*b1 + c1*x1*c0*c0*a1*a1 - c1*x1*c0*c0*b1*b1 - c1*x1*b0*b0*a1*a1 + c1*x1*a0*a0*b1*b1 + a1*z0*c0*c0*b1*b1 - a1*z0*b0*b0*c1*c1 - a1*z0*a0*a0*b1*b1 + a1*z0*a0*a0*c1*c1 - a1*z1*c0*c0*b1*b1 + a1*z1*b0*b0*c1*c1 + a1*z1*a0*a0*b1*b1 - a1*z1*a0*a0*c1*c1 - 2*a0* c1_3*y0*b0 + 2*a0* c1_3*y1*b0 + c1_3*x0*b0*b0 - c1_3*x0*a0*a0 - c1_3*x1*b0*b0 + c1_3*x1*a0*a0 + a1_3*z0*c0*c0 - 2*c1*c1*x1*c0*a1*a0 + 2*c1*c1*x1*c0*b1*b0 - 2*a1*a1*z0*c0*a0*c1 + 2*a1*a1*z0*b0*a0*b1 - a1_3*z0*b0*b0 - a1_3*z1*c0*c0 + a1_3*z1*b0*b0 + 2*a1*a1*z1*c0*a0*c1 - 2*a1*a1*z1*b0*a0*b1 + 2*c0*b1*b1*x0*a1*a0 - 2*c0*b1*x0*b0*a1*a1 - 2*c0*b1*b1*x1*a1*a0 + 2*c0*b1*x1*b0*a1*a1 + 2*a0*a0*c1*y0*a1*b1 - 2*a0*c1*y0*b0*a1*a1 + 2*a0*c1*c1*y0*c0*b1 - 2*a0*a0*c1*y1*a1*b1 + 2*a0*c1*y1*b0*a1*a1 - 2*a0*c1*c1*y1*c0*b1 - 2*c0*a1*a1*y0*a0*b1 + 2*c0* a1_3*y0*b0 - 2*c0* a1_3*y1*b0 + 2*c0*a1*y0*b0*c1*c1 - 2*c0*c0*a1*y0*c1*b1 + 2*c0*a1*a1*y1*a0*b1 - 2*c0*a1*y1*b0*c1*c1 + 2*c0*c0*a1*y1*c1*b1 + 2*a0*b1*z0*b0*c1*c1 - 2*a0*b1*b1*z0*c1*c0 - 2*a0*b1*z1*b0*c1*c1 + 2*a0*b1*b1*z1*c1*c0;
-
- Matrix diffh(1,6);
- diffh(0,0)=diffx0/K;
- diffh(0,1)=diffy0/K;
- diffh(0,2)=diffz0/K;
- K*=K;
- diffh(0,3)=diffa0/K;
- diffh(0,4)=diffb0/K;
- diffh(0,5)=0;
+ const double a1_3 = a1 * a1 * a1;
+ const double b1_3 = b1 * b1 * b1;
+ const double c1_3 = c1 * c1 * c1;
+
+ double K = c0 * c0 * a1 * a1 - 2 * c0 * a1 * a0 * c1 - 2 * c0 * b1 * b0 * c1 +
+ c0 * c0 * b1 * b1 - 2 * b0 * a1 * a0 * b1 + b0 * b0 * a1 * a1 +
+ b0 * b0 * c1 * c1 + a0 * a0 * b1 * b1 + a0 * a0 * c1 * c1;
+
+ const double diffx0 = -b0 * c1 + c0 * b1;
+ const double diffy0 = a0 * c1 - c0 * a1;
+ const double diffz0 = -a0 * b1 + b0 * a1;
+
+ const double diffa0 =
+ 2 * b0 * c1 * x0 * a0 * b1 * b1 + 2 * c0 * b1 * b1 * x0 * b0 * a1 +
+ 2 * c0 * c0 * b1 * x0 * c1 * a1 + 2 * c1 * c1 * y0 * c0 * a1 * a0 -
+ 2 * b0 * c1 * c1 * x0 * c0 * a1 - 2 * b0 * b0 * c1 * x0 * b1 * a1 -
+ 2 * c1 * c1 * y0 * c0 * b1 * b0 + 2 * b0 * b0 * c1 * x1 * b1 * a1 +
+ 2 * b0 * c1 * c1 * x1 * c0 * a1 - 2 * b0 * c1 * x1 * a0 * b1 * b1 -
+ c1 * y0 * c0 * c0 * a1 * a1 + c1 * y0 * c0 * c0 * b1 * b1 +
+ c1 * y0 * b0 * b0 * a1 * a1 - c1 * y0 * a0 * a0 * b1 * b1 +
+ c1 * y1 * c0 * c0 * a1 * a1 - c1 * y1 * c0 * c0 * b1 * b1 -
+ c1 * y1 * b0 * b0 * a1 * a1 + c1 * y1 * a0 * a0 * b1 * b1 -
+ b1 * z0 * c0 * c0 * a1 * a1 + b1 * z0 * b0 * b0 * a1 * a1 -
+ b1 * z0 * b0 * b0 * c1 * c1 + b1 * z0 * a0 * a0 * c1 * c1 +
+ b1 * z1 * c0 * c0 * a1 * a1 - b1 * z1 * b0 * b0 * a1 * a1 +
+ b1 * z1 * b0 * b0 * c1 * c1 - b1 * z1 * a0 * a0 * c1 * c1 +
+ 2 * b0 * c1_3 * x0 * a0 - 2 * b0 * c1_3 * x1 * a0 -
+ 2 * c0 * b1_3 * x0 * a0 + 2 * c0 * b1_3 * x1 * a0 + c1_3 * y0 * b0 * b0 -
+ c1_3 * y0 * a0 * a0 - c1_3 * y1 * b0 * b0 + c1_3 * y1 * a0 * a0 -
+ b1_3 * z0 * c0 * c0 + b1_3 * z0 * a0 * a0 + b1_3 * z1 * c0 * c0 -
+ b1_3 * z1 * a0 * a0 - 2 * c1 * c1 * y1 * c0 * a1 * a0 +
+ 2 * c1 * c1 * y1 * c0 * b1 * b0 + 2 * b1 * b1 * z0 * c0 * b0 * c1 -
+ 2 * b1 * b1 * z0 * b0 * a1 * a0 - 2 * b1 * b1 * z1 * c0 * b0 * c1 +
+ 2 * b1 * b1 * z1 * b0 * a1 * a0 - 2 * c0 * b1 * x0 * a0 * c1 * c1 -
+ 2 * c0 * b1 * b1 * x1 * b0 * a1 - 2 * c0 * c0 * b1 * x1 * c1 * a1 +
+ 2 * c0 * b1 * x1 * a0 * c1 * c1 + 2 * c0 * a1 * y0 * a0 * b1 * b1 -
+ 2 * c0 * a1 * a1 * y0 * b1 * b0 - 2 * c0 * a1 * y1 * a0 * b1 * b1 +
+ 2 * c0 * a1 * a1 * y1 * b1 * b0 + 2 * b0 * a1 * a1 * z0 * c1 * c0 -
+ 2 * b0 * a1 * z0 * a0 * c1 * c1 - 2 * b0 * a1 * a1 * z1 * c1 * c0 +
+ 2 * b0 * a1 * z1 * a0 * c1 * c1;
+ const double diffb0 =
+ -2 * c1 * c1 * x0 * c0 * b1 * b0 + 2 * c1 * c1 * x0 * c0 * a1 * a0 -
+ c1 * x0 * c0 * c0 * a1 * a1 + c1 * x0 * c0 * c0 * b1 * b1 +
+ c1 * x0 * b0 * b0 * a1 * a1 - c1 * x0 * a0 * a0 * b1 * b1 +
+ c1 * x1 * c0 * c0 * a1 * a1 - c1 * x1 * c0 * c0 * b1 * b1 -
+ c1 * x1 * b0 * b0 * a1 * a1 + c1 * x1 * a0 * a0 * b1 * b1 +
+ a1 * z0 * c0 * c0 * b1 * b1 - a1 * z0 * b0 * b0 * c1 * c1 -
+ a1 * z0 * a0 * a0 * b1 * b1 + a1 * z0 * a0 * a0 * c1 * c1 -
+ a1 * z1 * c0 * c0 * b1 * b1 + a1 * z1 * b0 * b0 * c1 * c1 +
+ a1 * z1 * a0 * a0 * b1 * b1 - a1 * z1 * a0 * a0 * c1 * c1 -
+ 2 * a0 * c1_3 * y0 * b0 + 2 * a0 * c1_3 * y1 * b0 + c1_3 * x0 * b0 * b0 -
+ c1_3 * x0 * a0 * a0 - c1_3 * x1 * b0 * b0 + c1_3 * x1 * a0 * a0 +
+ a1_3 * z0 * c0 * c0 - 2 * c1 * c1 * x1 * c0 * a1 * a0 +
+ 2 * c1 * c1 * x1 * c0 * b1 * b0 - 2 * a1 * a1 * z0 * c0 * a0 * c1 +
+ 2 * a1 * a1 * z0 * b0 * a0 * b1 - a1_3 * z0 * b0 * b0 -
+ a1_3 * z1 * c0 * c0 + a1_3 * z1 * b0 * b0 +
+ 2 * a1 * a1 * z1 * c0 * a0 * c1 - 2 * a1 * a1 * z1 * b0 * a0 * b1 +
+ 2 * c0 * b1 * b1 * x0 * a1 * a0 - 2 * c0 * b1 * x0 * b0 * a1 * a1 -
+ 2 * c0 * b1 * b1 * x1 * a1 * a0 + 2 * c0 * b1 * x1 * b0 * a1 * a1 +
+ 2 * a0 * a0 * c1 * y0 * a1 * b1 - 2 * a0 * c1 * y0 * b0 * a1 * a1 +
+ 2 * a0 * c1 * c1 * y0 * c0 * b1 - 2 * a0 * a0 * c1 * y1 * a1 * b1 +
+ 2 * a0 * c1 * y1 * b0 * a1 * a1 - 2 * a0 * c1 * c1 * y1 * c0 * b1 -
+ 2 * c0 * a1 * a1 * y0 * a0 * b1 + 2 * c0 * a1_3 * y0 * b0 -
+ 2 * c0 * a1_3 * y1 * b0 + 2 * c0 * a1 * y0 * b0 * c1 * c1 -
+ 2 * c0 * c0 * a1 * y0 * c1 * b1 + 2 * c0 * a1 * a1 * y1 * a0 * b1 -
+ 2 * c0 * a1 * y1 * b0 * c1 * c1 + 2 * c0 * c0 * a1 * y1 * c1 * b1 +
+ 2 * a0 * b1 * z0 * b0 * c1 * c1 - 2 * a0 * b1 * b1 * z0 * c1 * c0 -
+ 2 * a0 * b1 * z1 * b0 * c1 * c1 + 2 * a0 * b1 * b1 * z1 * c1 * c0;
+
+ Matrix diffh(1, 6);
+ diffh(0, 0) = diffx0 / K;
+ diffh(0, 1) = diffy0 / K;
+ diffh(0, 2) = diffz0 / K;
+ K *= K;
+ diffh(0, 3) = diffa0 / K;
+ diffh(0, 4) = diffb0 / K;
+ diffh(0, 5) = 0;
/* --- Multiply Jline=dline/dq with diffh=de/dline --- */
- J.resize(1,J.cols());
- J= diffh*Jline;
- //J=Jline;
+ J.resize(1, J.cols());
+ J = diffh * Jline;
+ // J=Jline;
-
- sotDEBUG(15)<<"# Out }"<<endl;
+ sotDEBUG(15) << "# Out }" << endl;
return J;
}
/** Compute the error between two visual features from a subset
-*a the possible features.
+ *a the possible features.
*/
-Vector&
-FeatureLineDistance::computeError( Vector& error,int time )
-{
+Vector &FeatureLineDistance::computeError(Vector &error, int time) {
sotDEBUGIN(15);
/* Line coordinates */
const Vector &line = lineSOUT(time);
- const double & x0 = line(0);
- const double & y0 = line(1);
- const double & z0 = line(2);
- const double & a0 = line(3);
- const double & b0 = line(4);
- const double & c0 = line(5);
+ const double &x0 = line(0);
+ const double &y0 = line(1);
+ const double &z0 = line(2);
+ const double &a0 = line(3);
+ const double &b0 = line(4);
+ const double &c0 = line(5);
const Vector &posRef = positionRefSIN(time);
- const double & x1 = posRef(0);
- const double & y1 = posRef(1);
- const double & z1 = posRef(2);
- const double & a1 = posRef(3);
- const double & b1 = posRef(4);
- const double & c1 = posRef(5);
+ const double &x1 = posRef(0);
+ const double &y1 = posRef(1);
+ const double &z1 = posRef(2);
+ const double &a1 = posRef(3);
+ const double &b1 = posRef(4);
+ const double &c1 = posRef(5);
error.resize(1);
- double K = c0*c0*a1*a1 - 2*c0*a1*a0*c1 - 2*c0*b1*b0*c1 + c0*c0*b1*b1 - 2*b0*a1*a0*b1 + b0*b0*a1*a1 + b0*b0*c1*c1 + a0*a0*b1*b1 + a0*a0*c1*c1;
- error(0) = ( - b0*c1*x0 + b0*c1*x1 + c0*b1*x0 - c0*b1*x1
- + a0*c1*y0 - a0*c1*y1 - c0*a1*y0 + c0*a1*y1
- - a0*b1*z0 + a0*b1*z1 + b0*a1*z0 - b0*a1*z1 ) / K;
-
+ double K = c0 * c0 * a1 * a1 - 2 * c0 * a1 * a0 * c1 - 2 * c0 * b1 * b0 * c1 +
+ c0 * c0 * b1 * b1 - 2 * b0 * a1 * a0 * b1 + b0 * b0 * a1 * a1 +
+ b0 * b0 * c1 * c1 + a0 * a0 * b1 * b1 + a0 * a0 * c1 * c1;
+ error(0) = (-b0 * c1 * x0 + b0 * c1 * x1 + c0 * b1 * x0 - c0 * b1 * x1 +
+ a0 * c1 * y0 - a0 * c1 * y1 - c0 * a1 * y0 + c0 * a1 * y1 -
+ a0 * b1 * z0 + a0 * b1 * z1 + b0 * a1 * z0 - b0 * a1 * z1) /
+ K;
/* --- DEBUG --- */
-// error.resize(6);
-// error=line-posRef;
+ // error.resize(6);
+ // error=line-posRef;
sotDEBUGOUT(15);
- return error ;
+ return error;
}
-void FeatureLineDistance::
-display( std::ostream& os ) const
-{
- os <<"LineDistance <"<<name<<">";
+void FeatureLineDistance::display(std::ostream &os) const {
+ os << "LineDistance <" << name << ">";
}
diff --git a/src/feature/feature-point6d-relative.cpp b/src/feature/feature-point6d-relative.cpp
index fda9a44b..e6ab8179 100644
--- a/src/feature/feature-point6d-relative.cpp
+++ b/src/feature/feature-point6d-relative.cpp
@@ -13,159 +13,164 @@
/* --- SOT --- */
#include <sot/core/debug.hh>
-#include <sot/core/feature-point6d-relative.hh>
#include <sot/core/exception-feature.hh>
+#include <sot/core/feature-point6d-relative.hh>
-#include <sot/core/matrix-geometry.hh>
-#include <dynamic-graph/pool.h>
#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/pool.h>
+#include <sot/core/matrix-geometry.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6dRelative,"FeaturePoint6dRelative");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6dRelative,
+ "FeaturePoint6dRelative");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-
-FeaturePoint6dRelative::
-FeaturePoint6dRelative( const string& pointName )
- : FeaturePoint6d( pointName )
- ,positionReferenceSIN( NULL,"sotFeaturePoint6dRelative("+name+")::input(matrixHomo)::positionRef" )
- ,articularJacobianReferenceSIN( NULL,"sotFeaturePoint6dRelative("+name+")::input(matrix)::JqRef" )
- ,dotpositionSIN(NULL,"sotFeaturePoint6dRelative("+name+")::input(matrixHomo)::dotposition" )
- ,dotpositionReferenceSIN(NULL,"sotFeaturePoint6dRelative("+name+")::input(matrixHomo)::dotpositionRef" )
-{
- jacobianSOUT.addDependency( positionReferenceSIN );
- jacobianSOUT.addDependency( articularJacobianReferenceSIN );
-
- errorSOUT.addDependency( positionReferenceSIN );
+FeaturePoint6dRelative::FeaturePoint6dRelative(const string &pointName)
+ : FeaturePoint6d(pointName),
+ positionReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
+ ")::input(matrixHomo)::positionRef"),
+ articularJacobianReferenceSIN(NULL, "sotFeaturePoint6dRelative(" + name +
+ ")::input(matrix)::JqRef"),
+ dotpositionSIN(NULL, "sotFeaturePoint6dRelative(" + name +
+ ")::input(matrixHomo)::dotposition"),
+ dotpositionReferenceSIN(NULL,
+ "sotFeaturePoint6dRelative(" + name +
+ ")::input(matrixHomo)::dotpositionRef") {
+ jacobianSOUT.addDependency(positionReferenceSIN);
+ jacobianSOUT.addDependency(articularJacobianReferenceSIN);
+
+ errorSOUT.addDependency(positionReferenceSIN);
errordotSOUT.addDependency(dotpositionReferenceSIN);
- signalRegistration( positionReferenceSIN<<articularJacobianReferenceSIN );
+ signalRegistration(positionReferenceSIN << articularJacobianReferenceSIN);
- signalRegistration( dotpositionSIN <<
- dotpositionReferenceSIN <<
- errordotSOUT );
+ signalRegistration(dotpositionSIN << dotpositionReferenceSIN << errordotSOUT);
- errordotSOUT.setFunction (boost::bind (&FeaturePoint6dRelative::computeErrordot,
- this, _1, _2));
+ errordotSOUT.setFunction(
+ boost::bind(&FeaturePoint6dRelative::computeErrordot, this, _1, _2));
initCommands();
}
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/** Compute the interaction matrix from a subset of
* the possible features.
*/
-Matrix& FeaturePoint6dRelative::
-computeJacobian( Matrix& Jres,int time )
-{
- sotDEBUG(15)<<"# In {"<<endl;
+Matrix &FeaturePoint6dRelative::computeJacobian(Matrix &Jres, int time) {
+ sotDEBUG(15) << "# In {" << endl;
- const Matrix & Jq = articularJacobianSIN(time);
- const Matrix & JqRef = articularJacobianReferenceSIN(time);
- const MatrixHomogeneous & wMp = positionSIN(time);
- const MatrixHomogeneous & wMpref = positionReferenceSIN(time);
+ const Matrix &Jq = articularJacobianSIN(time);
+ const Matrix &JqRef = articularJacobianReferenceSIN(time);
+ const MatrixHomogeneous &wMp = positionSIN(time);
+ const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
const Matrix::Index cJ = Jq.cols();
- Matrix J(6,cJ);
+ Matrix J(6, cJ);
{
- MatrixHomogeneous pMw; pMw = wMp.inverse(Eigen::Affine);
- MatrixHomogeneous pMpref; pMpref= pMw*wMpref;
-
- MatrixTwist pVpref; buildFrom(pMpref, pVpref );
- J = pVpref*JqRef;
+ MatrixHomogeneous pMw;
+ pMw = wMp.inverse(Eigen::Affine);
+ MatrixHomogeneous pMpref;
+ pMpref = pMw * wMpref;
+
+ MatrixTwist pVpref;
+ buildFrom(pMpref, pVpref);
+ J = pVpref * JqRef;
J -= Jq;
}
const Flags &fl = selectionSIN(time);
const int dim = dimensionSOUT(time);
- sotDEBUG(15) <<"Dimension="<<dim<<std::endl;
- Jres.resize(dim,cJ) ;
+ sotDEBUG(15) << "Dimension=" << dim << std::endl;
+ Jres.resize(dim, cJ);
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);
- rJ ++;
- }
-
- sotDEBUG(15)<<"# Out }"<<endl;
+ 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);
+ rJ++;
+ }
+
+ sotDEBUG(15) << "# Out }" << endl;
return Jres;
}
/** Compute the error between two visual features from a subset
* a the possible features.
*/
-Vector&
-FeaturePoint6dRelative::computeError( Vector& error,int time )
-{
+Vector &FeaturePoint6dRelative::computeError(Vector &error, int time) {
sotDEBUGIN(15);
-// /* TODO */
-// error.resize(6); error.fill(.0);
+ // /* TODO */
+ // error.resize(6); error.fill(.0);
- const MatrixHomogeneous & wMp = positionSIN(time);
- const MatrixHomogeneous & wMpref = positionReferenceSIN(time);
+ const MatrixHomogeneous &wMp = positionSIN(time);
+ const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
- MatrixHomogeneous pMw; pMw = wMp.inverse(Eigen::Affine);
- MatrixHomogeneous pMpref; pMpref = pMw*wMpref;
+ MatrixHomogeneous pMw;
+ pMw = wMp.inverse(Eigen::Affine);
+ MatrixHomogeneous pMpref;
+ pMpref = pMw * wMpref;
MatrixHomogeneous Merr;
- try
- {
- if( isReferenceSet() )
- {
- // TODO: Deal with the case of FeaturePoint6dRelative reference without dcast
- FeaturePoint6dRelative * sdes6d = dynamic_cast<FeaturePoint6dRelative*>(getReference());
- if( NULL!=sdes6d )
- {
- const MatrixHomogeneous & wMp_des = sdes6d->positionSIN(time);
- const MatrixHomogeneous & wMpref_des = sdes6d->positionReferenceSIN(time);
- MatrixHomogeneous pMw_des; pMw_des = wMp_des.inverse(Eigen::Affine);
- MatrixHomogeneous pMpref_des; pMpref_des=pMw_des*wMpref_des;
- MatrixHomogeneous Minv; Minv = pMpref_des.inverse(Eigen::Affine);
- Merr=pMpref*Minv;
- }
- else
- {
- const MatrixHomogeneous & Mref = getReference()->positionSIN(time);
- MatrixHomogeneous Minv; Minv = Mref.inverse(Eigen::Affine);
- Merr=pMpref*Minv;
- }
- }
- else
- {
- Merr=pMpref;
- }
- } catch( ... ) { Merr=pMpref; }
-
- MatrixRotation Rerr; Rerr = Merr.linear();
+ try {
+ if (isReferenceSet()) {
+ // TODO: Deal with the case of FeaturePoint6dRelative reference without
+ // dcast
+ FeaturePoint6dRelative *sdes6d =
+ dynamic_cast<FeaturePoint6dRelative *>(getReference());
+ if (NULL != sdes6d) {
+ const MatrixHomogeneous &wMp_des = sdes6d->positionSIN(time);
+ const MatrixHomogeneous &wMpref_des =
+ sdes6d->positionReferenceSIN(time);
+ MatrixHomogeneous pMw_des;
+ pMw_des = wMp_des.inverse(Eigen::Affine);
+ MatrixHomogeneous pMpref_des;
+ pMpref_des = pMw_des * wMpref_des;
+ MatrixHomogeneous Minv;
+ Minv = pMpref_des.inverse(Eigen::Affine);
+ Merr = pMpref * Minv;
+ } else {
+ const MatrixHomogeneous &Mref = getReference()->positionSIN(time);
+ MatrixHomogeneous Minv;
+ Minv = Mref.inverse(Eigen::Affine);
+ Merr = pMpref * Minv;
+ }
+ } else {
+ Merr = pMpref;
+ }
+ } catch (...) {
+ Merr = pMpref;
+ }
+
+ MatrixRotation Rerr;
+ Rerr = Merr.linear();
VectorUTheta rerr(Rerr);
const Flags &fl = selectionSIN(time);
- error.resize(dimensionSOUT(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); }
- for( unsigned int i=0;i<3;++i )
- { if( fl(i+3) ) error(cursor++) = rerr.angle()*rerr.axis()(i); }
+ for (unsigned int i = 0; i < 3; ++i) {
+ 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);
+ }
sotDEBUGOUT(15);
- return error ;
+ return error;
}
/** Compute the error between two visual features from a subset
@@ -173,121 +178,125 @@ FeaturePoint6dRelative::computeError( Vector& error,int time )
*
* This is computed by the desired feature.
*/
-Vector&
-FeaturePoint6dRelative::computeErrorDot( Vector& errordot,int time )
-{
+Vector &FeaturePoint6dRelative::computeErrorDot(Vector &errordot, int time) {
sotDEBUGIN(15);
// /* TODO */
// error.resize(6); error.fill(.0);
- const MatrixHomogeneous & wMp = positionSIN(time);
- const MatrixHomogeneous & wMpref = positionReferenceSIN(time);
- const MatrixHomogeneous & wdMp = dotpositionSIN(time);
- const MatrixHomogeneous & wdMpref = dotpositionReferenceSIN(time);
+ const MatrixHomogeneous &wMp = positionSIN(time);
+ const MatrixHomogeneous &wMpref = positionReferenceSIN(time);
+ const MatrixHomogeneous &wdMp = dotpositionSIN(time);
+ const MatrixHomogeneous &wdMpref = dotpositionReferenceSIN(time);
- sotDEBUG(15) << "wdMp :" <<wdMp << endl;
- sotDEBUG(15) << "wdMpref :" <<wdMpref << endl;
+ sotDEBUG(15) << "wdMp :" << wdMp << endl;
+ sotDEBUG(15) << "wdMpref :" << wdMpref << endl;
MatrixRotation dRerr;
Vector dtrerr;
- try
- {
- MatrixRotation wRp; wRp = wMp.linear();
- MatrixRotation wRpref; wRpref = wMpref.linear();
- MatrixRotation wdRp; wdRp = wdMp.linear();
- MatrixRotation wdRpref; wdRpref = wdMpref.linear();
-
- Vector trp(3); trp = wMp.translation();
- Vector trpref(3); trpref = wMpref.translation();
- Vector trdp(3); trdp = wdMp.translation();
- Vector trdpref(3); trdpref = wdMpref.translation();
-
- sotDEBUG(15) << "Everything is extracted" <<endl;
- MatrixRotation wdRpt,wRpt,op1,op2;
- wdRpt = wdRp.transpose();op1=wdRpt*wRpref;
- wRpt = wRp.transpose();op2=wRpt*wdRpref;
- dRerr = op1+op2;
-
- sotDEBUG(15) << "dRerr" << dRerr << endl;
- Vector trtmp1(3),vop1(3),vop2(3);
- trtmp1 = trpref-trp;
- vop1=wdRpt*trtmp1;
- trtmp1 = trdpref-trdp;
- vop2=wRpt*trtmp1;
- dtrerr = vop1-vop2;
-
- sotDEBUG(15) << "dtrerr" << dtrerr << endl;
-
-
- } catch( ... ) { sotDEBUG(15) << "You've got a problem with errordot." << std::endl; }
+ try {
+ MatrixRotation wRp;
+ wRp = wMp.linear();
+ MatrixRotation wRpref;
+ wRpref = wMpref.linear();
+ MatrixRotation wdRp;
+ wdRp = wdMp.linear();
+ MatrixRotation wdRpref;
+ wdRpref = wdMpref.linear();
+
+ Vector trp(3);
+ trp = wMp.translation();
+ Vector trpref(3);
+ trpref = wMpref.translation();
+ Vector trdp(3);
+ trdp = wdMp.translation();
+ Vector trdpref(3);
+ trdpref = wdMpref.translation();
+
+ sotDEBUG(15) << "Everything is extracted" << endl;
+ MatrixRotation wdRpt, wRpt, op1, op2;
+ wdRpt = wdRp.transpose();
+ op1 = wdRpt * wRpref;
+ wRpt = wRp.transpose();
+ op2 = wRpt * wdRpref;
+ dRerr = op1 + op2;
+
+ sotDEBUG(15) << "dRerr" << dRerr << endl;
+ Vector trtmp1(3), vop1(3), vop2(3);
+ trtmp1 = trpref - trp;
+ vop1 = wdRpt * trtmp1;
+ trtmp1 = trdpref - trdp;
+ vop2 = wRpt * trtmp1;
+ dtrerr = vop1 - vop2;
+
+ sotDEBUG(15) << "dtrerr" << dtrerr << endl;
+
+ } catch (...) {
+ sotDEBUG(15) << "You've got a problem with errordot." << std::endl;
+ }
VectorUTheta rerr(dRerr);
const Flags &fl = selectionSIN(time);
- errordot.resize(dimensionSOUT(time)) ;
+ errordot.resize(dimensionSOUT(time));
unsigned int cursor = 0;
- for( unsigned int i=0;i<3;++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); }
+ for (unsigned int i = 0; i < 3; ++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);
+ }
sotDEBUGOUT(15);
- return errordot ;
+ return errordot;
}
-static const char * featureNames []
-= { "X ",
- "Y ",
- "Z ",
- "RX",
- "RY",
- "RZ" };
-void FeaturePoint6dRelative::
-display( std::ostream& os ) const
-{
- os <<"Point6dRelative <"<<name<<">: (" ;
-
- try{
+static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
+void FeaturePoint6dRelative::display(std::ostream &os) const {
+ os << "Point6dRelative <" << name << ">: (";
+
+ try {
const Flags &fl = selectionSIN.accessCopy();
bool first = true;
- for( int i=0;i<6;++i )
- if( fl(i) )
- {
- if( first ) { first = false; } else { os << ","; }
- os << featureNames[i];
- }
- os<<") ";
- } catch(ExceptionAbstract e){ os<< " selectSIN not set."; }
+ for (int i = 0; i < 6; ++i)
+ if (fl(i)) {
+ if (first) {
+ first = false;
+ } else {
+ os << ",";
+ }
+ os << featureNames[i];
+ }
+ os << ") ";
+ } catch (ExceptionAbstract e) {
+ os << " selectSIN not set.";
+ }
}
-
-void FeaturePoint6dRelative::
-initCommands( void )
-{
+void FeaturePoint6dRelative::initCommands(void) {
using namespace command;
- addCommand( "initSdes",
- makeCommandVoid1( *this,&FeaturePoint6dRelative::initSdes,
- docCommandVoid1( "Initialize the desired feature.",
- "string (desired feature name)")));
+ addCommand("initSdes", makeCommandVoid1(
+ *this, &FeaturePoint6dRelative::initSdes,
+ docCommandVoid1("Initialize the desired feature.",
+ "string (desired feature name)")));
}
-/* Initialise the reference value: set up the sdes signal of the current feature,
- * and freezes the position and position-reference of the desired feature.
+/* Initialise the reference value: set up the sdes signal of the current
+ * feature, and freezes the position and position-reference of the desired
+ * feature.
*/
-void FeaturePoint6dRelative::
-initSdes( const std::string & nameSdes )
-{
- FeaturePoint6dRelative & sdes
- = dynamic_cast< FeaturePoint6dRelative &>
- (dg::PoolStorage::getInstance()->getEntity(nameSdes));
+void FeaturePoint6dRelative::initSdes(const std::string &nameSdes) {
+ FeaturePoint6dRelative &sdes = dynamic_cast<FeaturePoint6dRelative &>(
+ dg::PoolStorage::getInstance()->getEntity(nameSdes));
setReference(&sdes);
- const int timeCurr = positionSIN.getTime() +1;
- positionSIN.recompute( timeCurr );
- positionReferenceSIN.recompute( timeCurr );
+ const int timeCurr = positionSIN.getTime() + 1;
+ positionSIN.recompute(timeCurr);
+ positionReferenceSIN.recompute(timeCurr);
- sdes.positionSIN.setConstant( positionSIN.accessCopy() );
- sdes.positionReferenceSIN.setConstant( positionReferenceSIN.accessCopy() );
+ sdes.positionSIN.setConstant(positionSIN.accessCopy());
+ sdes.positionReferenceSIN.setConstant(positionReferenceSIN.accessCopy());
}
diff --git a/src/feature/feature-point6d.cpp b/src/feature/feature-point6d.cpp
index ed065f5a..5568faa4 100644
--- a/src/feature/feature-point6d.cpp
+++ b/src/feature/feature-point6d.cpp
@@ -16,55 +16,53 @@
/* --- SOT --- */
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/command-setter.h>
-#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-bind.h>
+#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command.h>
#include <Eigen/LU>
#include <sot/core/debug.hh>
-#include <sot/core/feature-point6d.hh>
#include <sot/core/exception-feature.hh>
+#include <sot/core/feature-point6d.hh>
using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6d,"FeaturePoint6d");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6d, "FeaturePoint6d");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-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" )
- ,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),
- accuracy_ (1e-8)
-{
- jacobianSOUT.addDependency( positionSIN );
- jacobianSOUT.addDependency( articularJacobianSIN );
-
- errorSOUT.addDependency( positionSIN );
-
- signalRegistration( positionSIN<<articularJacobianSIN );
- signalRegistration (errordotSOUT << velocitySIN);
- errordotSOUT.setFunction (boost::bind (&FeaturePoint6d::computeErrordot,
- this, _1, _2));
- errordotSOUT.addDependency (velocitySIN);
- errordotSOUT.addDependency (positionSIN);
- errordotSOUT.addDependency (errorSOUT);
+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"),
+ 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),
+ accuracy_(1e-8) {
+ jacobianSOUT.addDependency(positionSIN);
+ jacobianSOUT.addDependency(articularJacobianSIN);
+
+ errorSOUT.addDependency(positionSIN);
+
+ signalRegistration(positionSIN << articularJacobianSIN);
+ signalRegistration(errordotSOUT << velocitySIN);
+ errordotSOUT.setFunction(
+ boost::bind(&FeaturePoint6d::computeErrordot, this, _1, _2));
+ errordotSOUT.addDependency(velocitySIN);
+ errordotSOUT.addDependency(positionSIN);
+ errordotSOUT.addDependency(errorSOUT);
// Commands
//
@@ -72,226 +70,223 @@ 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";
+ 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";
addCommand("getFrame",
- new dynamicgraph::command::Getter<FeaturePoint6d, std::string>
- (*this, &FeaturePoint6d::computationFrame, docstring));
- addCommand("keep",
- makeCommandVoid0(*this,&FeaturePoint6d::servoCurrentPosition,
- docCommandVoid0("modify the desired position to servo at current pos.")));
+ new dynamicgraph::command::Getter<FeaturePoint6d, std::string>(
+ *this, &FeaturePoint6d::computationFrame, docstring));
+ addCommand(
+ "keep",
+ makeCommandVoid0(
+ *this, &FeaturePoint6d::servoCurrentPosition,
+ docCommandVoid0(
+ "modify the desired position to servo at current pos.")));
}
}
-void FeaturePoint6d::
-addDependenciesFromReference( void )
-{
- assert( isReferenceSet() );
- errorSOUT.addDependency( getReference()->positionSIN );
- jacobianSOUT.addDependency( getReference()->positionSIN );
+void FeaturePoint6d::addDependenciesFromReference(void) {
+ assert(isReferenceSet());
+ errorSOUT.addDependency(getReference()->positionSIN);
+ jacobianSOUT.addDependency(getReference()->positionSIN);
}
-void FeaturePoint6d::
-removeDependenciesFromReference( void )
-{
- assert( isReferenceSet() );
- errorSOUT.removeDependency( getReference()->positionSIN );
- jacobianSOUT.removeDependency( getReference()->positionSIN );
+void FeaturePoint6d::removeDependenciesFromReference(void) {
+ assert(isReferenceSet());
+ errorSOUT.removeDependency(getReference()->positionSIN);
+ jacobianSOUT.removeDependency(getReference()->positionSIN);
}
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void FeaturePoint6d::
-computationFrame(const std::string& inFrame)
-{
+void FeaturePoint6d::computationFrame(const std::string &inFrame) {
if (inFrame == "current")
computationFrame_ = FRAME_CURRENT;
else if (inFrame == "desired")
computationFrame_ = FRAME_DESIRED;
else {
- std::string msg("FeaturePoint6d::computationFrame: "
- + inFrame + ": invalid argument,\n"
- "expecting 'current' or 'desired'");
+ std::string msg("FeaturePoint6d::computationFrame: " + inFrame +
+ ": invalid argument,\n"
+ "expecting 'current' or 'desired'");
throw ExceptionFeature(ExceptionFeature::GENERIC, msg);
}
}
/// \brief Get computation frame
-std::string FeaturePoint6d::
-computationFrame() const
-{
- switch(computationFrame_)
- {
- case FRAME_CURRENT:
- return "current";
- case FRAME_DESIRED:
- return "desired";
- }
- assert( false&&"Case not handled" );
+std::string FeaturePoint6d::computationFrame() const {
+ switch (computationFrame_) {
+ case FRAME_CURRENT:
+ return "current";
+ case FRAME_DESIRED:
+ return "desired";
+ }
+ assert(false && "Case not handled");
return "Case not handled";
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned int& FeaturePoint6d::
-getDimension( unsigned int & dim, int time )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &FeaturePoint6d::getDimension(unsigned int &dim, int time) {
+ sotDEBUG(25) << "# In {" << endl;
const Flags &fl = selectionSIN.access(time);
dim = 0;
- for( int i=0;i<6;++i ) if( fl(i) ) dim++;
+ for (int i = 0; i < 6; ++i)
+ if (fl(i))
+ dim++;
- sotDEBUG(25)<<"# Out }"<<endl;
+ sotDEBUG(25) << "# Out }" << endl;
return dim;
}
-
/** Compute the interaction matrix from a subset of
* the possible features.
*/
-Matrix& FeaturePoint6d::
-computeJacobian( Matrix& J,int time )
-{
- sotDEBUG(15)<<"# In {"<<endl;
+Matrix &FeaturePoint6d::computeJacobian(Matrix &J, int time) {
+ sotDEBUG(15) << "# In {" << endl;
- const Matrix & Jq = articularJacobianSIN(time);
- const int & dim = dimensionSOUT(time);
+ const Matrix &Jq = articularJacobianSIN(time);
+ const int &dim = dimensionSOUT(time);
const Flags &fl = selectionSIN(time);
- sotDEBUG(25)<<"dim = "<<dimensionSOUT(time)<<" time:" << time << " "
- << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady() << endl;
- sotDEBUG(25)<<"selec = "<<selectionSIN(time)<<" time:" << time << " "
- << selectionSIN.getTime() << " " << selectionSIN.getReady() << endl;
+ sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " "
+ << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady()
+ << endl;
+ sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " "
+ << selectionSIN.getTime() << " " << selectionSIN.getReady()
+ << endl;
- sotDEBUG(15) <<"Dimension="<<dim<<std::endl;
+ sotDEBUG(15) << "Dimension=" << dim << std::endl;
const Matrix::Index cJ = Jq.cols();
- J.resize(dim,cJ) ;
- Matrix LJq(6,cJ);
-
- if( FRAME_CURRENT==computationFrame_ )
- {
- /* The Jacobian on rotation is equal to Jr = - hdRh Jr6d.
- * The Jacobian in translation is equalt to Jt = [hRw(wthd-wth)]x Jr - Jt. */
- const MatrixHomogeneous& wMh = positionSIN(time);
- MatrixRotation wRh; wRh = wMh.linear();
- MatrixRotation wRhd;
- Vector hdth(3),Rhdth(3);
-
- 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);
- }
- else
- {
- wRhd.setIdentity();
- for( unsigned int i=0;i<3;++i ) hdth(i)=-wMh(i,3);
- }
- Rhdth=(wRh.inverse())*hdth;
- MatrixRotation hdRh; hdRh = (wRhd.inverse())*wRh;
-
- Matrix Lx(6,6);
- for(unsigned int i=0;i<3;i++)
- {for(unsigned int j=0;j<3;j++)
- {
- if( i==j) { Lx(i,j)=-1; } else { Lx(i,j)=0; }
- Lx(i+3,j)=0;
- Lx(i+3,j+3)=-hdRh(i,j);
- }
- }
- const double & X=Rhdth(0), &Y=Rhdth(1), &Z=Rhdth(2);
- Lx(0,4) = -Z ; Lx(0,5) = Y ; Lx(1,3) = Z ;
- Lx(1,5) = -X ; Lx(2,3) = -Y ; Lx(2,4) = X ;
- Lx(0,3) = 0 ; Lx(1,4) = 0 ; Lx(2,5) = 0 ;
- sotDEBUG(15) << "Lx= "<<Lx<<endl;
-
- LJq=Lx*Jq;
+ J.resize(dim, cJ);
+ Matrix LJq(6, cJ);
+
+ if (FRAME_CURRENT == computationFrame_) {
+ /* The Jacobian on rotation is equal to Jr = - hdRh Jr6d.
+ * The Jacobian in translation is equalt to Jt = [hRw(wthd-wth)]x Jr - Jt.
+ */
+ const MatrixHomogeneous &wMh = positionSIN(time);
+ MatrixRotation wRh;
+ wRh = wMh.linear();
+ MatrixRotation wRhd;
+ Vector hdth(3), Rhdth(3);
+
+ 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);
+ } else {
+ wRhd.setIdentity();
+ for (unsigned int i = 0; i < 3; ++i)
+ hdth(i) = -wMh(i, 3);
}
- else
- {
- /* The Jacobian in rotation is equal to Jr = hdJ = hdRh Jr.
- * The Jacobian in translation is equal to Jr = hdJ = hdRh Jr. */
- const MatrixHomogeneous& wMh = positionSIN(time);
- MatrixRotation wRh; wRh = wMh.linear();
- MatrixRotation hdRh;
-
- if( isReferenceSet() )
- {
- const MatrixHomogeneous& wMhd = getReference()->positionSIN(time);
- MatrixRotation wRhd; wRhd = wMhd.linear();
- hdRh = (wRhd.inverse())*wRh;
+ Rhdth = (wRh.inverse()) * hdth;
+ MatrixRotation hdRh;
+ hdRh = (wRhd.inverse()) * wRh;
+
+ Matrix Lx(6, 6);
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ if (i == j) {
+ Lx(i, j) = -1;
+ } else {
+ Lx(i, j) = 0;
}
- else
- { hdRh = wRh; }
-
- LJq.fill(0);
- for(unsigned int i=0;i<3;i++)
- for(unsigned int j=0;j<cJ;j++)
- {
- for(unsigned int k=0;k<3;k++)
- {
- LJq(i,j)+=hdRh(i,k)*Jq(k,j);
- LJq(i+3,j)+=hdRh(i,k)*Jq(k+3,j);
- }
- }
+ Lx(i + 3, j) = 0;
+ Lx(i + 3, j + 3) = -hdRh(i, j);
+ }
+ }
+ const double &X = Rhdth(0), &Y = Rhdth(1), &Z = Rhdth(2);
+ Lx(0, 4) = -Z;
+ Lx(0, 5) = Y;
+ Lx(1, 3) = Z;
+ Lx(1, 5) = -X;
+ Lx(2, 3) = -Y;
+ Lx(2, 4) = X;
+ Lx(0, 3) = 0;
+ Lx(1, 4) = 0;
+ Lx(2, 5) = 0;
+ sotDEBUG(15) << "Lx= " << Lx << endl;
+
+ LJq = Lx * Jq;
+ } else {
+ /* The Jacobian in rotation is equal to Jr = hdJ = hdRh Jr.
+ * The Jacobian in translation is equal to Jr = hdJ = hdRh Jr. */
+ const MatrixHomogeneous &wMh = positionSIN(time);
+ MatrixRotation wRh;
+ wRh = wMh.linear();
+ MatrixRotation hdRh;
+
+ if (isReferenceSet()) {
+ const MatrixHomogeneous &wMhd = getReference()->positionSIN(time);
+ MatrixRotation wRhd;
+ wRhd = wMhd.linear();
+ hdRh = (wRhd.inverse()) * wRh;
+ } else {
+ hdRh = wRh;
}
+ LJq.fill(0);
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < cJ; j++) {
+ for (unsigned int k = 0; k < 3; k++) {
+ LJq(i, j) += hdRh(i, k) * Jq(k, j);
+ LJq(i + 3, j) += hdRh(i, k) * Jq(k + 3, j);
+ }
+ }
+ }
+
/* Select the active line of Jq. */
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);
- rJ ++;
- }
+ 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);
+ rJ++;
+ }
- sotDEBUG(15)<<"# Out }"<<endl;
+ sotDEBUG(15) << "# Out }" << endl;
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
* a the possible features.
*/
-Vector&
-FeaturePoint6d::computeError( Vector& error,int time )
-{
+Vector &FeaturePoint6d::computeError(Vector &error, int time) {
sotDEBUGIN(15);
const Flags &fl = selectionSIN(time);
- const MatrixHomogeneous& wMh = positionSIN(time);
- sotDEBUG(15)<<"wMh = "<<wMh<<endl;
+ const MatrixHomogeneous &wMh = positionSIN(time);
+ sotDEBUG(15) << "wMh = " << wMh << endl;
/* Computing only translation: *
* trans( hMw wMhd ) = htw + hRw wthd *
@@ -300,115 +295,148 @@ FeaturePoint6d::computeError( Vector& error,int time )
* The second line is obtained by writting hMw as the inverse of wMh. */
MatrixHomogeneous hMhd;
- if(isReferenceSet())
- {
- 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.
- };
- }
- else
- {
- switch(computationFrame_)
- {
- case FRAME_CURRENT:
- hMhd=wMh.inverse(); break;
- case FRAME_DESIRED:
- hMhd=wMh; break; // Compute hdMh indeed.
- };
- }
+ if (isReferenceSet()) {
+ 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.
+ };
+ } else {
+ switch (computationFrame_) {
+ case FRAME_CURRENT:
+ hMhd = wMh.inverse();
+ break;
+ case FRAME_DESIRED:
+ hMhd = wMh;
+ break; // Compute hdMh indeed.
+ };
+ }
- sotDEBUG(25)<<"dim = "<<dimensionSOUT(time)<<" time:" << time << " "
- << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady() << endl;
- sotDEBUG(25)<<"selec = "<<selectionSIN(time)<<" time:" << time << " "
- << selectionSIN.getTime() << " " << selectionSIN.getReady() << endl;
+ sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " "
+ << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady()
+ << endl;
+ sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " "
+ << selectionSIN.getTime() << " " << selectionSIN.getReady()
+ << endl;
- error.resize(dimensionSOUT(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(3)||fl(4)||fl(5))
- {
- MatrixRotation hRhd; 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);
- }
+ for (unsigned int i = 0; i < 3; ++i) {
+ if (fl(i))
+ error(cursor++) = hMhd(i, 3);
+ }
+
+ if (fl(3) || fl(4) || fl(5)) {
+ MatrixRotation hRhd;
+ 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);
}
+ }
sotDEBUGOUT(15);
- return error ;
+ return error;
}
-void FeaturePoint6d::inverseJacobianRodrigues ()
-{
- const double& r1 = error_th_.angle()*error_th_.axis()(0);
- const double& r2 = error_th_.angle()*error_th_.axis()(1);
- const double& r3 = error_th_.angle()*error_th_.axis()(2);
- double r1_2 = r1*r1;
- double r2_2 = r2*r2;
- double r3_2 = r3*r3;
- double r1_3 = r1*r1_2;
- double r2_3 = r2*r2_2;
- double r3_3 = r3*r3_2;
- double r1_4 = r1_2*r1_2;
- double r2_4 = r2_2*r2_2;
- double r3_4 = r3_2*r3_2;
- double norm_2 = r3_2+r2_2+r1_2;
-
- if (norm_2 < accuracy_) {
- P_.setIdentity ();
- }
- else {
- // This code has been generated by maxima software
- P_ (0,0) = ((r3_2+r2_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r1_2*r3_2+r1_2*r2_2+r1_4)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (0,1) = -(r1*r2*sqrt(norm_2)*sin(sqrt(norm_2))+(r3_3+(r2_2+r1_2)*r3)*cos(sqrt(norm_2))-r3_3-r1*r2*r3_2+(-r2_2-r1_2)*r3-r1*r2_3-r1_3*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (0,2) = -(r1*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(-r2*r3_2-r2_3-r1_2*r2)*cos(sqrt(norm_2))-r1*r3_3+r2*r3_2+(-r1*r2_2-r1_3)*r3+r2_3+r1_2*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (1,0) = -(r1*r2*sqrt(norm_2)*sin(sqrt(norm_2))+((-r2_2-r1_2)*r3-r3_3)*cos(sqrt(norm_2))+r3_3-r1*r2*r3_2+(r2_2+r1_2)*r3-r1*r2_3-r1_3*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (1,1) = ((r3_2+r1_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r2_2*r3_2+r2_4+r1_2*r2_2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (1,2) = -(r2*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(r1*r3_2+r1*r2_2+r1_3)*cos(sqrt(norm_2))-r2*r3_3-r1*r3_2+(-r2_3-r1_2*r2)*r3-r1*r2_2-r1_3)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (2,0) = -(r1*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(r2*r3_2+r2_3+r1_2*r2)*cos(sqrt(norm_2))-r1*r3_3-r2*r3_2+(-r1*r2_2-r1_3)*r3-r2_3-r1_2*r2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (2,1) = -(r2*r3*sqrt(norm_2)*sin(sqrt(norm_2))+(-r1*r3_2-r1*r2_2-r1_3)*cos(sqrt(norm_2))-r2*r3_3+r1*r3_2+(-r2_3-r1_2*r2)*r3+r1*r2_2+r1_3)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- P_ (2,2) = ((r2_2+r1_2)*sqrt(norm_2)*sin(sqrt(norm_2))+r3_4+(r2_2+r1_2)*r3_2)/(r3_4+(2*r2_2+2*r1_2)*r3_2+r2_4+2*r1_2*r2_2+r1_4);
- }
- Pinv_= P_.inverse ();
+void FeaturePoint6d::inverseJacobianRodrigues() {
+ const double &r1 = error_th_.angle() * error_th_.axis()(0);
+ const double &r2 = error_th_.angle() * error_th_.axis()(1);
+ const double &r3 = error_th_.angle() * error_th_.axis()(2);
+ double r1_2 = r1 * r1;
+ double r2_2 = r2 * r2;
+ double r3_2 = r3 * r3;
+ double r1_3 = r1 * r1_2;
+ double r2_3 = r2 * r2_2;
+ double r3_3 = r3 * r3_2;
+ double r1_4 = r1_2 * r1_2;
+ double r2_4 = r2_2 * r2_2;
+ double r3_4 = r3_2 * r3_2;
+ double norm_2 = r3_2 + r2_2 + r1_2;
+
+ if (norm_2 < accuracy_) {
+ P_.setIdentity();
+ } else {
+ // This code has been generated by maxima software
+ P_(0, 0) =
+ ((r3_2 + r2_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r1_2 * r3_2 +
+ r1_2 * r2_2 + r1_4) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(0, 1) =
+ -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) +
+ (r3_3 + (r2_2 + r1_2) * r3) * cos(sqrt(norm_2)) - r3_3 -
+ r1 * r2 * r3_2 + (-r2_2 - r1_2) * r3 - r1 * r2_3 - r1_3 * r2) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(0, 2) =
+ -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
+ (-r2 * r3_2 - r2_3 - r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 +
+ r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 + r2_3 + r1_2 * r2) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(1, 0) =
+ -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) +
+ ((-r2_2 - r1_2) * r3 - r3_3) * cos(sqrt(norm_2)) + r3_3 -
+ r1 * r2 * r3_2 + (r2_2 + r1_2) * r3 - r1 * r2_3 - r1_3 * r2) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(1, 1) =
+ ((r3_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r2_2 * r3_2 + r2_4 +
+ r1_2 * r2_2) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(1, 2) =
+ -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
+ (r1 * r3_2 + r1 * r2_2 + r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 -
+ r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 - r1 * r2_2 - r1_3) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(2, 0) =
+ -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
+ (r2 * r3_2 + r2_3 + r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 -
+ r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 - r2_3 - r1_2 * r2) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(2, 1) =
+ -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) +
+ (-r1 * r3_2 - r1 * r2_2 - r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 +
+ r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 + r1 * r2_2 + r1_3) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ P_(2, 2) =
+ ((r2_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r3_4 +
+ (r2_2 + r1_2) * r3_2) /
+ (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4);
+ }
+ Pinv_ = P_.inverse();
}
-Vector& FeaturePoint6d::computeErrordot( Vector& errordot,int time )
-{
- if(isReferenceSet()) {
- const Vector& velocity = getReference()->velocitySIN(time);
- const MatrixHomogeneous& M = positionSIN (time);
- const MatrixHomogeneous& Mref = getReference()->positionSIN (time);
+Vector &FeaturePoint6d::computeErrordot(Vector &errordot, int time) {
+ if (isReferenceSet()) {
+ const Vector &velocity = getReference()->velocitySIN(time);
+ const MatrixHomogeneous &M = positionSIN(time);
+ const MatrixHomogeneous &Mref = getReference()->positionSIN(time);
// Linear velocity if the reference frame
- v_ (0) = velocity (0);
- v_ (1) = velocity (1);
- v_ (2) = velocity (2);
+ v_(0) = velocity(0);
+ v_(1) = velocity(1);
+ v_(2) = velocity(2);
// Angular velocity if the reference frame
- omega_ (0) = velocity (3);
- omega_ (1) = velocity (4);
- omega_ (2) = velocity (5);
+ omega_(0) = velocity(3);
+ omega_(1) = velocity(4);
+ omega_(2) = velocity(5);
R_ = M.linear();
t_ = M.translation();
- Rt_ = R_.transpose ();
+ Rt_ = R_.transpose();
Rref_ = Mref.linear();
- tref_= Mref.translation();
- Rreft_ = Rref_.transpose ();
- errorSOUT.recompute (time);
- inverseJacobianRodrigues ();
+ tref_ = Mref.translation();
+ Rreft_ = Rref_.transpose();
+ errorSOUT.recompute(time);
+ inverseJacobianRodrigues();
switch (computationFrame_) {
case FRAME_CURRENT:
// \dot{e}_{t} = R^{T} v
- errordot_t_ = Rt_* v_;
+ errordot_t_ = Rt_ * v_;
// \dot{e}_{\theta} = P^{-1}(e_{theta})R^{*T}\omega
- Rreftomega_ = Rreft_*omega_;
+ Rreftomega_ = Rreft_ * omega_;
errordot_th_ = Pinv_ * Rreftomega_;
break;
case FRAME_DESIRED:
@@ -417,23 +445,23 @@ Vector& FeaturePoint6d::computeErrordot( Vector& errordot,int time )
break;
}
} else {
- errordot_t_.setZero ();
- errordot_th_.setZero ();
+ errordot_t_.setZero();
+ errordot_th_.setZero();
}
const Flags &fl = selectionSIN(time);
errordot.resize(dimensionSOUT(time));
unsigned int cursor = 0;
- for( unsigned int i=0;i<3;++i ) {
- if( fl(i) ) {
- errordot (cursor++) = errordot_t_ (i);
+ for (unsigned int i = 0; i < 3; ++i) {
+ if (fl(i)) {
+ errordot(cursor++) = errordot_t_(i);
}
}
- if(fl(3)||fl(4)||fl(5)) {
- for( unsigned int i=0;i<3;++i ) {
- if( fl(i+3) ) {
- errordot (cursor++) = errordot_th_ (i);
+ if (fl(3) || fl(4) || fl(5)) {
+ for (unsigned int i = 0; i < 3; ++i) {
+ if (fl(i + 3)) {
+ errordot(cursor++) = errordot_th_(i);
}
}
}
@@ -441,47 +469,42 @@ Vector& FeaturePoint6d::computeErrordot( Vector& errordot,int time )
return errordot;
}
-
/* Modify the value of the reference (sdes) so that it corresponds
* to the current position. The effect on the servo is to maintain the
* current position and correct any drift. */
-void FeaturePoint6d::
-servoCurrentPosition( void )
-{
+void FeaturePoint6d::servoCurrentPosition(void) {
sotDEBUGIN(15);
- if(! isReferenceSet() )
- {
- sotERROR << "The reference is not set, this function should not be called" <<std::endl;
- throw ExceptionFeature(ExceptionFeature::GENERIC,
- "The reference is not set, this function should not be called");
- }
+ if (!isReferenceSet()) {
+ sotERROR << "The reference is not set, this function should not be called"
+ << std::endl;
+ throw ExceptionFeature(
+ ExceptionFeature::GENERIC,
+ "The reference is not set, this function should not be called");
+ }
getReference()->positionSIN = positionSIN.accessCopy();
sotDEBUGOUT(15);
}
-static const char * featureNames []
-= { "X ",
- "Y ",
- "Z ",
- "RX",
- "RY",
- "RZ" };
-void FeaturePoint6d::
-display( std::ostream& os ) const
-{
- os <<"Point6d <"<<name<<">: (" ;
-
- try{
+static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
+void FeaturePoint6d::display(std::ostream &os) const {
+ os << "Point6d <" << name << ">: (";
+
+ try {
const Flags &fl = selectionSIN.accessCopy();
bool first = true;
- for( int i=0;i<6;++i )
- if( fl(i) )
- {
- if( first ) { first = false; } else { os << ","; }
- os << featureNames[i];
- }
- os<<") ";
- } catch(ExceptionAbstract e){ os<< " selectSIN not set."; }
+ for (int i = 0; i < 6; ++i)
+ if (fl(i)) {
+ if (first) {
+ first = false;
+ } else {
+ os << ",";
+ }
+ os << featureNames[i];
+ }
+ os << ") ";
+ } catch (ExceptionAbstract e) {
+ os << " selectSIN not set.";
+ }
}
diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp
index b76027a0..0550890f 100644
--- a/src/feature/feature-pose.cpp
+++ b/src/feature/feature-pose.cpp
@@ -13,99 +13,103 @@
/* --- SOT --- */
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
-#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>
+#include <boost/type_traits/is_same.hpp>
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/command-setter.h>
-#include <dynamic-graph/command-getter.h>
#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 <sot/core/debug.hh>
-#include <sot/core/feature-pose.hh>
#include <sot/core/factory.hh>
+#include <sot/core/feature-pose.hh>
using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-typedef pinocchio::CartesianProductOperation <
- pinocchio::VectorSpaceOperationTpl<3, double>,
- pinocchio::SpecialOrthogonalOperationTpl<3, double>
- > R3xSO3_t;
+typedef pinocchio::CartesianProductOperation<
+ pinocchio::VectorSpaceOperationTpl<3, double>,
+ pinocchio::SpecialOrthogonalOperationTpl<3, double>>
+ R3xSO3_t;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
-template <Representation_t representation>
-struct LG_t
-{
- typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t, R3xSO3_t>::type type;
+template <Representation_t representation> struct LG_t {
+ typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
+ R3xSO3_t>::type type;
};
typedef FeaturePose<R3xSO3Representation> FeaturePose_t;
-typedef FeaturePose<SE3Representation > FeaturePoseSE3_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");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-static const MatrixHomogeneous Id (MatrixHomogeneous::Identity());
+static const MatrixHomogeneous Id(MatrixHomogeneous::Identity());
template <Representation_t representation>
-FeaturePose<representation>::
-FeaturePose( const string& pointName )
- : FeatureAbstract( pointName )
- , oMja ( NULL,CLASS_NAME+"("+name+")::input(matrixHomo)::oMja" )
- , jaMfa ( NULL,CLASS_NAME+"("+name+")::input(matrixHomo)::jaMfa")
- , oMjb ( NULL,CLASS_NAME+"("+name+")::input(matrixHomo)::oMjb" )
- , jbMfb ( NULL,CLASS_NAME+"("+name+")::input(matrixHomo)::jbMfb")
- , jaJja ( NULL,CLASS_NAME+"("+name+")::input(matrix)::jaJja")
- , jbJjb ( NULL,CLASS_NAME+"("+name+")::input(matrix)::jbJjb")
-
- , faMfbDes ( NULL,CLASS_NAME+"("+name+")::input(matrixHomo)::faMfbDes")
- , faNufafbDes ( NULL,CLASS_NAME+"("+name+")::input(vector)::faNufafbDes")
-
- , faMfb (boost::bind (&FeaturePose<representation>::computefaMfb, this, _1, _2),
- oMja << jaMfa << oMjb << jbMfb,
- CLASS_NAME+"("+name+")::output(vector7)::q_faMfbDes")
- , q_faMfb (boost::bind (&FeaturePose<representation>::computeQfaMfb, this, _1, _2),
- faMfb,
- CLASS_NAME+"("+name+")::output(vector7)::q_faMfb")
- , q_faMfbDes (boost::bind (&FeaturePose<representation>::computeQfaMfbDes, this, _1, _2),
- faMfbDes,
- CLASS_NAME+"("+name+")::output(vector7)::q_faMfbDes")
-{
- oMja.setConstant (Id);
- jaMfa.setConstant (Id);
- jbMfb.setConstant (Id);
- faMfbDes.setConstant (Id);
- faNufafbDes.setConstant (Vector::Zero(6));
-
- jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb
- << jaJja << jbJjb );
-
- errorSOUT.addDependencies( q_faMfbDes << q_faMfb );
-
- signalRegistration( oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb );
- signalRegistration (errordotSOUT << faMfbDes << faNufafbDes);
-
- errordotSOUT.setFunction (boost::bind (&FeaturePose<representation>::computeErrorDot,
- this, _1, _2));
- errordotSOUT.addDependencies (q_faMfbDes << q_faMfb << faNufafbDes);
+FeaturePose<representation>::FeaturePose(const string &pointName)
+ : FeatureAbstract(pointName),
+ oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"),
+ jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"),
+ oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"),
+ jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"),
+ jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"),
+ jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb")
+
+ ,
+ faMfbDes(NULL,
+ CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"),
+ faNufafbDes(NULL,
+ CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes")
+
+ ,
+ faMfb(
+ boost::bind(&FeaturePose<representation>::computefaMfb, this, _1, _2),
+ oMja << jaMfa << oMjb << jbMfb,
+ CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes"),
+ q_faMfb(boost::bind(&FeaturePose<representation>::computeQfaMfb, this, _1,
+ _2),
+ faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"),
+ q_faMfbDes(boost::bind(&FeaturePose<representation>::computeQfaMfbDes,
+ this, _1, _2),
+ faMfbDes,
+ CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") {
+ oMja.setConstant(Id);
+ jaMfa.setConstant(Id);
+ jbMfb.setConstant(Id);
+ faMfbDes.setConstant(Id);
+ faNufafbDes.setConstant(Vector::Zero(6));
+
+ jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb);
+
+ errorSOUT.addDependencies(q_faMfbDes << q_faMfb);
+
+ signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb);
+ signalRegistration(errordotSOUT << faMfbDes << faNufafbDes);
+
+ errordotSOUT.setFunction(
+ boost::bind(&FeaturePose<representation>::computeErrorDot, this, _1, _2));
+ errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes);
// Commands
//
{
using namespace dynamicgraph::command;
- addCommand("keep",
- makeCommandVoid0(*this,&FeaturePose<representation>::servoCurrentPosition,
- docCommandVoid0("modify the desired position to servo at current pos.")));
+ addCommand(
+ "keep",
+ makeCommandVoid0(
+ *this, &FeaturePose<representation>::servoCurrentPosition,
+ docCommandVoid0(
+ "modify the desired position to servo at current pos.")));
}
}
@@ -114,95 +118,96 @@ FeaturePose( const string& pointName )
/* --------------------------------------------------------------------- */
template <Representation_t representation>
-static inline void check (const FeaturePose<representation>& ft)
-{
+static inline void check(const FeaturePose<representation> &ft) {
(void)ft;
- assert (ft.oMja .isPlugged() );
- assert (ft.jaMfa.isPlugged() );
- assert (ft.oMjb .isPlugged() );
- assert (ft.jbMfb.isPlugged() );
- assert (ft.faMfbDes .isPlugged() );
- assert (ft.faNufafbDes.isPlugged() );
+ assert(ft.oMja.isPlugged());
+ assert(ft.jaMfa.isPlugged());
+ assert(ft.oMjb.isPlugged());
+ assert(ft.jbMfb.isPlugged());
+ assert(ft.faMfbDes.isPlugged());
+ assert(ft.faNufafbDes.isPlugged());
}
template <Representation_t representation>
-unsigned int& FeaturePose<representation>::
-getDimension( unsigned int & dim, int time )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &FeaturePose<representation>::getDimension(unsigned int &dim,
+ int time) {
+ sotDEBUG(25) << "# In {" << endl;
const Flags &fl = selectionSIN.access(time);
dim = 0;
- for( int i=0;i<6;++i ) if( fl(i) ) dim++;
+ for (int i = 0; i < 6; ++i)
+ if (fl(i))
+ dim++;
- sotDEBUG(25)<<"# Out }"<<endl;
+ sotDEBUG(25) << "# Out }" << endl;
return dim;
}
-void toVector (const MatrixHomogeneous& M, Vector7& v)
-{
+void toVector(const MatrixHomogeneous &M, Vector7 &v) {
v.head<3>() = M.translation();
QuaternionMap(v.tail<4>().data()) = M.linear();
}
-Vector7 toVector (const MatrixHomogeneous& M)
-{
+Vector7 toVector(const MatrixHomogeneous &M) {
Vector7 ret;
- toVector (M, ret);
+ toVector(M, ret);
return ret;
}
template <Representation_t representation>
-Matrix& FeaturePose<representation>::computeJacobian( Matrix& J,int time )
-{
+Matrix &FeaturePose<representation>::computeJacobian(Matrix &J, int time) {
typedef typename LG_t<representation>::type LieGroup_t;
check(*this);
- q_faMfb .recompute(time);
+ q_faMfb.recompute(time);
q_faMfbDes.recompute(time);
- const int & dim = dimensionSOUT(time);
+ const int &dim = dimensionSOUT(time);
const Flags &fl = selectionSIN(time);
- const Matrix & _jbJjb = jbJjb (time);
+ const Matrix &_jbJjb = jbJjb(time);
- const MatrixHomogeneous& _jbMfb = (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
+ const MatrixHomogeneous &_jbMfb =
+ (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
const Matrix::Index cJ = _jbJjb.cols();
- J.resize(dim,cJ) ;
+ J.resize(dim, cJ);
MatrixTwist X;
- Eigen::Matrix<double,6,6,Eigen::RowMajor> Jminus;
+ Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
- buildFrom (_jbMfb.inverse(Eigen::Affine), X);
- MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() * oMja.access(time).rotation().transpose()
- * oMjb.access(time).rotation() * _jbMfb.rotation();
+ buildFrom(_jbMfb.inverse(Eigen::Affine), X);
+ MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() *
+ oMja.access(time).rotation().transpose() *
+ oMjb.access(time).rotation() * _jbMfb.rotation();
if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
- X.topRows<3>().applyOnTheLeft (faRfb);
- LieGroup_t().template dDifference<pinocchio::ARG1>(q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
+ X.topRows<3>().applyOnTheLeft(faRfb);
+ LieGroup_t().template dDifference<pinocchio::ARG1>(
+ q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
// Contribution of b:
// J = Jminus * X * jbJjb;
unsigned int rJ = 0;
- for( unsigned int r=0;r<6;++r )
- if( fl(r) )
+ for (unsigned int r = 0; r < 6; ++r)
+ if (fl(r))
J.row(rJ++) = (Jminus * X).row(r) * _jbJjb;
if (jaJja.isPlugged()) {
- const Matrix & _jaJja = jaJja (time);
- const MatrixHomogeneous& _jaMfa = (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
- _faMfb = faMfb.accessCopy();
+ const Matrix &_jaJja = jaJja(time);
+ const MatrixHomogeneous &_jaMfa =
+ (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
+ _faMfb = faMfb.accessCopy();
- buildFrom ((_jaMfa *_faMfb).inverse(Eigen::Affine), X);
+ buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X);
if (boost::is_same<LieGroup_t, R3xSO3_t>::value)
- X.topRows<3>().applyOnTheLeft (faRfb);
+ X.topRows<3>().applyOnTheLeft(faRfb);
// J -= (Jminus * X) * jaJja(time);
rJ = 0;
- for( unsigned int r=0;r<6;++r )
- if( fl(r) )
+ for (unsigned int r = 0; r < 6; ++r)
+ if (fl(r))
J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja;
}
@@ -210,73 +215,76 @@ Matrix& FeaturePose<representation>::computeJacobian( Matrix& J,int time )
}
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) * jbMfb(time);
+ res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) *
+ jbMfb(time);
return res;
}
template <Representation_t representation>
-Vector7& FeaturePose<representation>::computeQfaMfb (Vector7& res, int time)
-{
+Vector7 &FeaturePose<representation>::computeQfaMfb(Vector7 &res, int time) {
check(*this);
- toVector (faMfb(time), res);
+ toVector(faMfb(time), res);
return res;
}
template <Representation_t representation>
-Vector7& FeaturePose<representation>::computeQfaMfbDes (Vector7& res, int time)
-{
+Vector7 &FeaturePose<representation>::computeQfaMfbDes(Vector7 &res, int time) {
check(*this);
- toVector (faMfbDes (time), res);
+ toVector(faMfbDes(time), res);
return res;
}
template <Representation_t representation>
-Vector& FeaturePose<representation>::computeError( Vector& error,int time )
-{
+Vector &FeaturePose<representation>::computeError(Vector &error, int time) {
typedef typename LG_t<representation>::type LieGroup_t;
check(*this);
const Flags &fl = selectionSIN(time);
- Eigen::Matrix<double,6,1> v;
- LieGroup_t().difference (q_faMfbDes(time), q_faMfb(time), v);
+ Eigen::Matrix<double, 6, 1> v;
+ LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v);
- error.resize(dimensionSOUT(time)) ;
+ error.resize(dimensionSOUT(time));
unsigned int cursor = 0;
- for( unsigned int i=0;i<6;++i )
- if( fl(i) )
+ for (unsigned int i = 0; i < 6; ++i)
+ if (fl(i))
error(cursor++) = v(i);
- return error ;
+ return error;
}
// This function is responsible of converting the input velocity expressed with
// SE(3) convention onto a velocity expressed with the convention of this
// feature (R^3xSO(3) or SE(3)), in the right frame.
-template <typename LG_t> Vector6d convertVelocity (const MatrixHomogeneous& M, const MatrixHomogeneous& Mdes, const Vector& faNufafbDes)
-{
+template <typename LG_t>
+Vector6d convertVelocity(const MatrixHomogeneous &M,
+ const MatrixHomogeneous &Mdes,
+ const Vector &faNufafbDes) {
(void)M;
MatrixTwist X;
- buildFrom (Mdes.inverse(Eigen::Affine), X);
+ buildFrom(Mdes.inverse(Eigen::Affine), X);
return X * faNufafbDes;
}
-template <> Vector6d convertVelocity<R3xSO3_t> (const MatrixHomogeneous& M, const MatrixHomogeneous& Mdes, const Vector& faNufafbDes)
-{
+template <>
+Vector6d convertVelocity<R3xSO3_t>(const MatrixHomogeneous &M,
+ const MatrixHomogeneous &Mdes,
+ const Vector &faNufafbDes) {
Vector6d nu;
- nu.head<3>() = faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
+ nu.head<3>() =
+ faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>());
nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>();
return nu;
}
template <Representation_t representation>
-Vector& FeaturePose<representation>::computeErrorDot( Vector& errordot,int time )
-{
+Vector &FeaturePose<representation>::computeErrorDot(Vector &errordot,
+ int time) {
typedef typename LG_t<representation>::type LieGroup_t;
check(*this);
@@ -287,18 +295,20 @@ Vector& FeaturePose<representation>::computeErrorDot( Vector& errordot,int time
return errordot;
}
- q_faMfb .recompute(time);
+ q_faMfb.recompute(time);
q_faMfbDes.recompute(time);
- const MatrixHomogeneous& _faMfbDes = faMfbDes(time);
+ const MatrixHomogeneous &_faMfbDes = faMfbDes(time);
- Eigen::Matrix<double,6,6,Eigen::RowMajor> Jminus;
+ Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Jminus;
- LieGroup_t().template dDifference<pinocchio::ARG0>(q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
- Vector6d nu = convertVelocity<LieGroup_t> (faMfb(time), _faMfbDes, faNufafbDes.accessCopy());
+ LieGroup_t().template dDifference<pinocchio::ARG0>(
+ q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus);
+ Vector6d nu = convertVelocity<LieGroup_t>(faMfb(time), _faMfbDes,
+ faNufafbDes.accessCopy());
unsigned int cursor = 0;
- for( unsigned int i=0;i<6;++i )
- if( fl(i) )
+ for (unsigned int i = 0; i < 6; ++i)
+ if (fl(i))
errordot(cursor++) = Jminus.row(i) * nu;
return errordot;
@@ -308,40 +318,37 @@ Vector& FeaturePose<representation>::computeErrorDot( Vector& errordot,int time
* to the current position. The effect on the servo is to maintain the
* current position and correct any drift. */
template <Representation_t representation>
-void FeaturePose<representation>::
-servoCurrentPosition( void )
-{
+void FeaturePose<representation>::servoCurrentPosition(void) {
check(*this);
- const MatrixHomogeneous& _oMja = (oMja .isPlugged() ? oMja .accessCopy() : Id),
- _jaMfa = (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
- _oMjb = oMjb .accessCopy(),
- _jbMfb = (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
+ const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.accessCopy() : Id),
+ _jaMfa =
+ (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id),
+ _oMjb = oMjb.accessCopy(),
+ _jbMfb =
+ (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id);
faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb;
}
-static const char * featureNames []
-= { "X ",
- "Y ",
- "Z ",
- "RX",
- "RY",
- "RZ" };
+static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"};
template <Representation_t representation>
-void FeaturePose<representation>::
-display( std::ostream& os ) const
-{
- os <<CLASS_NAME<<"<"<<name<<">: (" ;
+void FeaturePose<representation>::display(std::ostream &os) const {
+ os << CLASS_NAME << "<" << name << ">: (";
- try{
+ try {
const Flags &fl = selectionSIN.accessCopy();
bool first = true;
- for( int i=0;i<6;++i )
- if( fl(i) )
- {
- if( first ) { first = false; } else { os << ","; }
- os << featureNames[i];
- }
- os<<") ";
- } catch(ExceptionAbstract e){ os<< " selectSIN not set."; }
+ for (int i = 0; i < 6; ++i)
+ if (fl(i)) {
+ if (first) {
+ first = false;
+ } else {
+ os << ",";
+ }
+ os << featureNames[i];
+ }
+ os << ") ";
+ } catch (ExceptionAbstract e) {
+ os << " selectSIN not set.";
+ }
}
diff --git a/src/feature/feature-posture.cpp b/src/feature/feature-posture.cpp
index 711ac23f..a911897e 100644
--- a/src/feature/feature-posture.cpp
+++ b/src/feature/feature-posture.cpp
@@ -1,176 +1,156 @@
// Copyright 2010, François Bleibel, Thomas Moulard, Olivier Stasse,
// JRL, CNRS/AIST.
-#include <string>
#include <boost/assign/list_of.hpp>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
+#include <string>
-#include <sot/core/feature-posture.hh>
#include <boost/numeric/conversion/cast.hpp>
+#include <sot/core/feature-posture.hh>
namespace dg = ::dynamicgraph;
using ::dynamicgraph::command::Setter;
using dynamicgraph::sot::FeatureAbstract;
namespace dynamicgraph {
- namespace sot {
- using command::Command;
- using command::Value;
-
- class FeaturePosture::SelectDof : public Command {
- public:
- virtual ~SelectDof () {}
- SelectDof(FeaturePosture& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::UNSIGNED)
- (Value::BOOL), docstring) {}
- virtual Value doExecute()
- {
- FeaturePosture& feature = static_cast<FeaturePosture&>(owner());
- std::vector<Value> values = getParameterValues();
- unsigned int dofId = values[0].value();
- bool control = values[1].value();
- feature.selectDof (dofId, control);
- return Value ();
- }
- }; // 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"),
- jacobian_(),
- activeDofs_ (),
- nbActiveDofs_ (0)
- {
- signalRegistration (state_ << posture_ << postureDot_);
-
- errorSOUT.addDependency (state_);
-
- 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";
- addCommand("selectDof", new SelectDof(*this, docstring));
+namespace sot {
+using command::Command;
+using command::Value;
+
+class FeaturePosture::SelectDof : public Command {
+public:
+ virtual ~SelectDof() {}
+ SelectDof(FeaturePosture &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::UNSIGNED)(Value::BOOL),
+ docstring) {}
+ virtual Value doExecute() {
+ FeaturePosture &feature = static_cast<FeaturePosture &>(owner());
+ std::vector<Value> values = getParameterValues();
+ unsigned int dofId = values[0].value();
+ bool control = values[1].value();
+ feature.selectDof(dofId, control);
+ return Value();
+ }
+}; // 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"),
+ jacobian_(), activeDofs_(), nbActiveDofs_(0) {
+ signalRegistration(state_ << posture_ << postureDot_);
+
+ errorSOUT.addDependency(state_);
+
+ 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";
+ addCommand("selectDof", new SelectDof(*this, docstring));
+}
+
+FeaturePosture::~FeaturePosture() {}
+
+unsigned int &FeaturePosture::getDimension(unsigned int &res, int) {
+ res = static_cast<unsigned int>(nbActiveDofs_);
+ return res;
+}
+
+dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
+ const dg::Vector &state = state_.access(t);
+ const dg::Vector &posture = posture_.access(t);
+
+ res.resize(nbActiveDofs_);
+ std::size_t index = 0;
+ for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
+ if (activeDofs_[i]) {
+ res(index) = state(i) - posture(i);
+ index++;
}
-
- FeaturePosture::~FeaturePosture ()
- {
+ }
+ return res;
+}
+
+dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &res, int) {
+ res = jacobian_;
+ return res;
+}
+
+dg::Vector &FeaturePosture::computeActivation(dg::Vector &res, int) {
+ return res;
+}
+
+dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
+ const Vector &postureDot = postureDot_.access(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);
+ }
+ return res;
+}
+
+void FeaturePosture::selectDof(unsigned dofId, bool control) {
+ const Vector &state = state_.accessCopy();
+ const Vector &posture = posture_.accessCopy();
+ std::size_t dim(state.size());
+
+ if (dim != (std::size_t)posture.size()) {
+ throw std::runtime_error("Posture and State should have same dimension.");
+ }
+ // If activeDof_ vector not initialized, initialize it
+ if (activeDofs_.size() != dim) {
+ activeDofs_ = std::vector<bool>(dim, false);
+ nbActiveDofs_ = 0;
+ }
+
+ // Check that selected dof id is valid
+ if ((dofId < 6) || (dofId >= dim)) {
+ std::ostringstream oss;
+ oss << "dof id should be more than 5 and less than state "
+ "dimension: "
+ << dim << ". Received " << dofId << ".";
+ throw ExceptionAbstract(ExceptionAbstract::TOOLS, oss.str());
+ }
+
+ if (control) {
+ if (!activeDofs_[dofId]) {
+ activeDofs_[dofId] = true;
+ nbActiveDofs_++;
}
-
-
- unsigned int& FeaturePosture::getDimension( unsigned int& res,int )
- {
- res = static_cast <unsigned int> (nbActiveDofs_);
- return res;
- }
-
- dg::Vector& FeaturePosture::computeError( dg::Vector& res, int t)
- {
- const dg::Vector& state = state_.access (t);
- const dg::Vector& posture = posture_.access (t);
-
- res.resize (nbActiveDofs_);
- std::size_t index=0;
- for (std::size_t i=0; i<activeDofs_.size (); ++i) {
- if (activeDofs_ [i]) {
- res (index) = state (i) - posture (i);
- index ++;
- }
- }
- return res;
+ } else { // control = false
+ if (activeDofs_[dofId]) {
+ activeDofs_[dofId] = false;
+ nbActiveDofs_--;
}
-
- dg::Matrix& FeaturePosture::computeJacobian( dg::Matrix& res, int )
- {
- res = jacobian_;
- return res;
- }
-
- dg::Vector& FeaturePosture::computeActivation( dg::Vector& res, int )
- {
- return res;
- }
-
- dg::Vector& FeaturePosture::computeErrorDot( dg::Vector& res, int t)
- {
- const Vector& postureDot = postureDot_.access (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);
- }
- return res;
- }
-
- void
- FeaturePosture::selectDof (unsigned dofId, bool control)
- {
- const Vector& state = state_.accessCopy();
- const Vector& posture = posture_.accessCopy ();
- std::size_t dim (state.size());
-
- if (dim != (std::size_t)posture.size ()) {
- throw std::runtime_error
- ("Posture and State should have same dimension.");
- }
- // If activeDof_ vector not initialized, initialize it
- if (activeDofs_.size () != dim) {
- activeDofs_ = std::vector <bool> (dim, false);
- nbActiveDofs_ = 0;
- }
-
- // Check that selected dof id is valid
- if ((dofId < 6) || (dofId >= dim))
- {
- std::ostringstream oss;
- oss << "dof id should be more than 5 and less than state "
- "dimension: "
- << dim << ". Received " << dofId << ".";
- throw ExceptionAbstract(ExceptionAbstract::TOOLS,
- oss.str());
- }
-
- if (control) {
- if (!activeDofs_ [dofId]) {
- activeDofs_ [dofId] = true;
- nbActiveDofs_ ++;
- }
- }
- else { // control = false
- if (activeDofs_ [dofId]) {
- activeDofs_ [dofId] = false;
- nbActiveDofs_ --;
- }
- }
- // recompute jacobian
- jacobian_.resize (nbActiveDofs_, dim);
- jacobian_.setZero ();
-
- std::size_t index=0;
- for (std::size_t i=0; i<activeDofs_.size (); ++i) {
- if (activeDofs_ [i]) {
- jacobian_ (index, i) = 1;
- index ++;
- }
- }
-
+ }
+ // recompute jacobian
+ jacobian_.resize(nbActiveDofs_, dim);
+ jacobian_.setZero();
+
+ std::size_t index = 0;
+ for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
+ if (activeDofs_[i]) {
+ jacobian_(index, i) = 1;
+ index++;
}
+ }
+}
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture");
- } // namespace sot
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture");
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/feature/feature-task.cpp b/src/feature/feature-task.cpp
index 208ea13a..bb6b7290 100644
--- a/src/feature/feature-task.cpp
+++ b/src/feature/feature-task.cpp
@@ -12,30 +12,23 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
+#include <dynamic-graph/pool.h>
#include <sot/core/debug.hh>
-#include <sot/core/feature-task.hh>
#include <sot/core/exception-feature.hh>
+#include <sot/core/feature-task.hh>
#include <sot/core/task.hh>
-#include <dynamic-graph/pool.h>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureTask,"FeatureTask");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureTask, "FeatureTask");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-
-FeatureTask::
-FeatureTask( const string& pointName )
- : FeatureGeneric( pointName )
-{
-}
+FeatureTask::FeatureTask(const string &pointName) : FeatureGeneric(pointName) {}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -45,10 +38,9 @@ FeatureTask( const string& pointName )
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void FeatureTask::
-display( std::ostream& os ) const
-{
+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 db76a75a..9bfefa4c 100644
--- a/src/feature/feature-vector3.cpp
+++ b/src/feature/feature-vector3.cpp
@@ -15,117 +15,117 @@
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <sot/core/debug.hh>
-#include <sot/core/feature-vector3.hh>
#include <sot/core/exception-feature.hh>
+#include <sot/core/feature-vector3.hh>
-#include <sot/core/matrix-geometry.hh>
#include <sot/core/factory.hh>
+#include <sot/core/matrix-geometry.hh>
using namespace dynamicgraph::sot;
using namespace std;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVector3,"FeatureVector3");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVector3, "FeatureVector3");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-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" )
-{
- jacobianSOUT.addDependency( positionSIN );
- jacobianSOUT.addDependency( articularJacobianSIN );
-
- errorSOUT.addDependency( vectorSIN );
- errorSOUT.addDependency( positionSIN );
- errorSOUT.addDependency( positionRefSIN );
-
- signalRegistration( vectorSIN<<positionSIN
- <<articularJacobianSIN<<positionRefSIN );
+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") {
+ jacobianSOUT.addDependency(positionSIN);
+ jacobianSOUT.addDependency(articularJacobianSIN);
+
+ errorSOUT.addDependency(vectorSIN);
+ errorSOUT.addDependency(positionSIN);
+ errorSOUT.addDependency(positionRefSIN);
+
+ signalRegistration(vectorSIN << positionSIN << articularJacobianSIN
+ << positionRefSIN);
}
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned int& FeatureVector3::
-getDimension( unsigned int & dim, int /*time*/ )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &FeatureVector3::getDimension(unsigned int &dim, int /*time*/) {
+ sotDEBUG(25) << "# In {" << endl;
- return dim=3;
+ return dim = 3;
}
-
-
/* --------------------------------------------------------------------- */
/** Compute the interaction matrix from a subset of
* the possible features.
*/
-Matrix& FeatureVector3::
-computeJacobian( Matrix& J,int time )
-{
- sotDEBUG(15)<<"# In {"<<endl;
-
- const Matrix & Jq = articularJacobianSIN(time);
- const Vector & vect = vectorSIN(time);
- const MatrixHomogeneous & M = positionSIN(time);
- MatrixRotation R; R = M.linear();
-
- Matrix Skew(3,3);
- Skew( 0,0 ) = 0 ; Skew( 0,1 )=-vect( 2 ); Skew( 0,2 ) = vect( 1 );
- Skew( 1,0 ) = vect( 2 ); Skew( 1,1 )= 0 ; Skew( 1,2 ) =-vect( 0 );
- Skew( 2,0 ) =-vect( 1 ); Skew( 2,1 )= vect( 0 ); Skew( 2,2 ) = 0 ;
-
- Matrix RSk(3,3); RSk = R*Skew;
-
- J.resize(3,Jq.cols());
- for( unsigned int i=0;i<3;++i )
- for( int j=0;j<Jq.cols();++j )
- {
- J(i,j)=0;
- for( unsigned int k=0;k<3;++k )
- { J(i,j)-=RSk(i,k)*Jq(k+3,j); }
+Matrix &FeatureVector3::computeJacobian(Matrix &J, int time) {
+ sotDEBUG(15) << "# In {" << endl;
+
+ const Matrix &Jq = articularJacobianSIN(time);
+ const Vector &vect = vectorSIN(time);
+ const MatrixHomogeneous &M = positionSIN(time);
+ MatrixRotation R;
+ R = M.linear();
+
+ Matrix Skew(3, 3);
+ Skew(0, 0) = 0;
+ Skew(0, 1) = -vect(2);
+ Skew(0, 2) = vect(1);
+ Skew(1, 0) = vect(2);
+ Skew(1, 1) = 0;
+ Skew(1, 2) = -vect(0);
+ Skew(2, 0) = -vect(1);
+ Skew(2, 1) = vect(0);
+ Skew(2, 2) = 0;
+
+ Matrix RSk(3, 3);
+ RSk = R * Skew;
+
+ J.resize(3, Jq.cols());
+ for (unsigned int i = 0; i < 3; ++i)
+ for (int j = 0; j < Jq.cols(); ++j) {
+ J(i, j) = 0;
+ for (unsigned int k = 0; k < 3; ++k) {
+ J(i, j) -= RSk(i, k) * Jq(k + 3, j);
}
+ }
- sotDEBUG(15)<<"# Out }"<<endl;
+ sotDEBUG(15) << "# Out }" << endl;
return J;
}
/** Compute the error between two visual features from a subset
-*a the possible features.
+ *a the possible features.
*/
-Vector&
-FeatureVector3::computeError( Vector& Mvect3,int time )
-{
+Vector &FeatureVector3::computeError(Vector &Mvect3, int time) {
sotDEBUGIN(15);
- const MatrixHomogeneous & M = positionSIN(time);
- const Vector & vect = vectorSIN(time);
- const Vector & vectdes = positionRefSIN(time);
+ const MatrixHomogeneous &M = positionSIN(time);
+ const Vector &vect = vectorSIN(time);
+ const Vector &vectdes = positionRefSIN(time);
sotDEBUG(15) << "M = " << M << std::endl;
sotDEBUG(15) << "v = " << vect << std::endl;
sotDEBUG(15) << "vd = " << vectdes << std::endl;
- MatrixRotation R; R = M.linear();
+ MatrixRotation R;
+ R = M.linear();
Mvect3.resize(3);
- Mvect3 = R*vect;
+ Mvect3 = R * vect;
Mvect3 -= vectdes;
sotDEBUGOUT(15);
- return Mvect3 ;
+ return Mvect3;
}
-void FeatureVector3::
-display( std::ostream& os ) const
-{
- os <<"Vector3 <"<<name<<">";
+void FeatureVector3::display(std::ostream &os) const {
+ os << "Vector3 <" << name << ">";
}
diff --git a/src/feature/feature-visual-point.cpp b/src/feature/feature-visual-point.cpp
index 06a5e5cf..c768f6c4 100644
--- a/src/feature/feature-visual-point.cpp
+++ b/src/feature/feature-visual-point.cpp
@@ -12,182 +12,176 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
-#include <sot/core/feature-visual-point.hh>
-#include <sot/core/exception-feature.hh>
#include <sot/core/debug.hh>
+#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
+#include <sot/core/feature-visual-point.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVisualPoint,"FeatureVisualPoint");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureVisualPoint, "FeatureVisualPoint");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+FeatureVisualPoint::FeatureVisualPoint(const string &pointName)
+ : FeatureAbstract(pointName), L(),
+ xySIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(vector)::xy"),
+ ZSIN(NULL, "sotFeatureVisualPoint(" + name + ")::input(double)::Z"),
+ articularJacobianSIN(NULL, "sotFeatureVisualPoint(" + name +
+ ")::input(matrix)::Jq") {
+ ZSIN = 1.;
+ jacobianSOUT.addDependency(xySIN);
+ jacobianSOUT.addDependency(ZSIN);
+ jacobianSOUT.addDependency(articularJacobianSIN);
-FeatureVisualPoint::
-FeatureVisualPoint( const string& pointName )
- : FeatureAbstract( pointName )
- ,L()
- ,xySIN( NULL,"sotFeatureVisualPoint("+name+")::input(vector)::xy" )
- ,ZSIN( NULL,"sotFeatureVisualPoint("+name+")::input(double)::Z" )
- ,articularJacobianSIN( NULL,"sotFeatureVisualPoint("+name+")::input(matrix)::Jq" )
-{
- ZSIN=1.;
-
- jacobianSOUT.addDependency( xySIN );
- jacobianSOUT.addDependency( ZSIN );
- jacobianSOUT.addDependency( articularJacobianSIN );
+ errorSOUT.addDependency(xySIN);
+ errorSOUT.addDependency(ZSIN);
- errorSOUT.addDependency( xySIN );
- errorSOUT.addDependency( ZSIN );
-
- signalRegistration( xySIN<<ZSIN<<articularJacobianSIN );
+ signalRegistration(xySIN << ZSIN << articularJacobianSIN);
}
-void FeatureVisualPoint::addDependenciesFromReference( void )
-{
- assert( isReferenceSet() );
- errorSOUT.addDependency( getReference()->xySIN );
+void FeatureVisualPoint::addDependenciesFromReference(void) {
+ assert(isReferenceSet());
+ errorSOUT.addDependency(getReference()->xySIN);
}
-void FeatureVisualPoint::removeDependenciesFromReference( void )
-{
- assert( isReferenceSet() );
- errorSOUT.removeDependency( getReference()->xySIN );
+void FeatureVisualPoint::removeDependenciesFromReference(void) {
+ assert(isReferenceSet());
+ errorSOUT.removeDependency(getReference()->xySIN);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned int& FeatureVisualPoint::
-getDimension( unsigned int & dim, int time )
-{
- sotDEBUG(25)<<"# In {"<<endl;
+unsigned int &FeatureVisualPoint::getDimension(unsigned int &dim, int time) {
+ sotDEBUG(25) << "# In {" << endl;
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;
+ sotDEBUG(25) << "# Out }" << endl;
return dim;
}
-
/** Compute the interaction matrix from a subset of
* the possible features.
*/
-Matrix& FeatureVisualPoint::
-computeJacobian( Matrix& J,int time )
-{
- sotDEBUG(15)<<"# In {"<<endl;
+Matrix &FeatureVisualPoint::computeJacobian(Matrix &J, int time) {
+ sotDEBUG(15) << "# In {" << endl;
sotDEBUG(15) << "Get selection flags." << endl;
const Flags &fl = selectionSIN(time);
const int dim = dimensionSOUT(time);
- L.resize(dim,6) ;
+ L.resize(dim, 6);
unsigned int cursorL = 0;
- sotDEBUG(5)<<std::endl;
-
- const double & Z = ZSIN(time);
- sotDEBUG(5)<<xySIN(time)<<std::endl;
- const double & x = xySIN(time)(0);
- const double & y = xySIN(time)(1);
+ sotDEBUG(5) << std::endl;
+ const double &Z = ZSIN(time);
+ sotDEBUG(5) << xySIN(time) << std::endl;
+ const double &x = xySIN(time)(0);
+ const double &y = xySIN(time)(1);
- if( Z<0 )
- { throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
- "VisualPoint is behind the camera"," (Z=%.1f).",Z)); }
+ if (Z < 0) {
+ throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
+ "VisualPoint is behind the camera", " (Z=%.1f).",
+ Z));
+ }
- if( fabs(Z)<1e-6 )
- { throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
- "VisualPoint Z coordinates is null"," (Z=%.3f)",Z)); }
+ if (fabs(Z) < 1e-6) {
+ throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
+ "VisualPoint Z coordinates is null", " (Z=%.3f)",
+ Z));
+ }
- if( fl(0) )
- {
- L(cursorL,0) = -1/Z ;
- L(cursorL,1) = 0 ;
- L(cursorL,2) = x/Z ;
- L(cursorL,3) = x*y ;
- L(cursorL,4) = -(1+x*x) ;
- L(cursorL,5) = y ;
+ if (fl(0)) {
+ L(cursorL, 0) = -1 / Z;
+ L(cursorL, 1) = 0;
+ L(cursorL, 2) = x / Z;
+ L(cursorL, 3) = x * y;
+ L(cursorL, 4) = -(1 + x * x);
+ L(cursorL, 5) = y;
- cursorL++;
- }
+ cursorL++;
+ }
- if( fl(1) )
- {
- L(cursorL,0) = 0 ;
- L(cursorL,1) = -1/Z ;
- L(cursorL,2) = y/Z ;
- L(cursorL,3) = 1+y*y ;
- L(cursorL,4) = -x*y ;
- L(cursorL,5) = -x ;
+ if (fl(1)) {
+ L(cursorL, 0) = 0;
+ L(cursorL, 1) = -1 / Z;
+ L(cursorL, 2) = y / Z;
+ L(cursorL, 3) = 1 + y * y;
+ L(cursorL, 4) = -x * y;
+ L(cursorL, 5) = -x;
cursorL++;
}
- sotDEBUG(15) << "L:"<<endl<<L<<endl;
- sotDEBUG(15) << "Jq:"<<endl<<articularJacobianSIN(time)<<endl;
+ sotDEBUG(15) << "L:" << endl << L << endl;
+ sotDEBUG(15) << "Jq:" << endl << articularJacobianSIN(time) << endl;
- J = L*articularJacobianSIN(time);
+ J = L * articularJacobianSIN(time);
- sotDEBUG(15)<<"# Out }"<<endl;
+ sotDEBUG(15) << "# Out }" << endl;
return J;
}
/** Compute the error between two visual features from a subset
* a the possible features.
*/
-Vector&
-FeatureVisualPoint::computeError( Vector& error,int time )
-{
+Vector &FeatureVisualPoint::computeError(Vector &error, int time) {
const Flags &fl = selectionSIN(time);
sotDEBUGIN(15);
- error.resize(dimensionSOUT(time)) ;
+ error.resize(dimensionSOUT(time));
unsigned int cursorL = 0;
- if(! isReferenceSet() )
- { throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
- "S* is not of adequate type.")); }
+ if (!isReferenceSet()) {
+ throw(ExceptionFeature(ExceptionFeature::BAD_INIT,
+ "S* is not of adequate type."));
+ }
- if( fl(0) )
- { error( cursorL++ ) = xySIN(time)(0) - getReference()->xySIN(time)(0) ; }
- if( fl(1) )
- { error( cursorL++ ) = xySIN(time)(1) - getReference()->xySIN(time)(1) ; }
+ if (fl(0)) {
+ error(cursorL++) = xySIN(time)(0) - getReference()->xySIN(time)(0);
+ }
+ if (fl(1)) {
+ error(cursorL++) = xySIN(time)(1) - getReference()->xySIN(time)(1);
+ }
sotDEBUGOUT(15);
- return error ;
+ return error;
}
-void FeatureVisualPoint::
-display( std::ostream& os ) const
-{
- os <<"VisualPoint <"<<name<<">:";
+void FeatureVisualPoint::display(std::ostream &os) const {
+ os << "VisualPoint <" << name << ">:";
- 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) ;
- } catch(ExceptionAbstract e){ os<< " XY or select not set."; }
+ 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);
+ } catch (ExceptionAbstract e) {
+ os << " XY or select not set.";
+ }
try {
- const double& z = ZSIN.accessCopy ();
- os<<" Z=" << z << " ";
- }catch(ExceptionAbstract e){ os<< " Z not set."; }
+ const double &z = ZSIN.accessCopy();
+ os << " Z=" << z << " ";
+ } catch (ExceptionAbstract e) {
+ os << " Z not set.";
+ }
}
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/feature/visual-point-projecter.cpp b/src/feature/visual-point-projecter.cpp
index 0bd3041d..4a8e19c6 100644
--- a/src/feature/visual-point-projecter.cpp
+++ b/src/feature/visual-point-projecter.cpp
@@ -3,92 +3,88 @@
*
*/
-#include <sot/core/visual-point-projecter.hh>
-#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
+#include <sot/core/debug.hh>
+#include <sot/core/visual-point-projecter.hh>
-namespace dynamicgraph
-{
- namespace sot
- {
-
- namespace dg = ::dynamicgraph;
- using namespace dg;
-
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VisualPointProjecter,"VisualPointProjecter");
-
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- VisualPointProjecter::
- VisualPointProjecter( const std::string & name )
- : Entity(name)
-
- ,CONSTRUCT_SIGNAL_IN(point3D,dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN(transfo,MatrixHomogeneous)
-
- ,CONSTRUCT_SIGNAL_OUT(point3Dgaze,dynamicgraph::Vector,m_point3DSIN<<m_transfoSIN )
- ,CONSTRUCT_SIGNAL_OUT(depth,double,m_point3DgazeSOUT )
- ,CONSTRUCT_SIGNAL_OUT(point2D,dynamicgraph::Vector,m_point3DgazeSOUT<<m_depthSOUT )
- {
- Entity::signalRegistration( m_point3DSIN );
- Entity::signalRegistration( m_transfoSIN );
- Entity::signalRegistration( m_point3DgazeSOUT );
- Entity::signalRegistration( m_point2DSOUT );
- Entity::signalRegistration( m_depthSOUT );
- }
-
-
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
-
- 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; Mi = M.inverse(Eigen::Affine);
- p3g = Mi.matrix()*p3;
- return p3g;
- }
-
- dynamicgraph::Vector& VisualPointProjecter::
- point2DSOUT_function( dynamicgraph::Vector &p2, int iter )
- {
- sotDEBUGIN(15);
-
- const dynamicgraph::Vector & p3 = m_point3DgazeSOUT(iter);
- const double &z =m_depthSOUT(iter);
- assert(z>0);
-
- p2.resize(2);
- p2(0) = p3(0) / z;
- p2(1) = p3(1) / z;
-
- sotDEBUGOUT(15);
- return p2;
- }
-
- double& VisualPointProjecter::
- depthSOUT_function( double & z, int iter )
- {
- const dynamicgraph::Vector & p3 = m_point3DgazeSOUT(iter);
- assert(p3.size() == 3);
- z=p3(2);
- return z;
- }
-
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
- /* --- ENTITY ----------------------------------------------------------- */
-
- void VisualPointProjecter::
- display( std::ostream& os ) const
- {
- os << "VisualPointProjecter "<<getName();
- }
-
- } // namespace sot
+namespace dynamicgraph {
+namespace sot {
+
+namespace dg = ::dynamicgraph;
+using namespace dg;
+
+/* --- DG FACTORY ------------------------------------------------------- */
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VisualPointProjecter,
+ "VisualPointProjecter");
+
+/* --- CONSTRUCTION ----------------------------------------------------- */
+/* --- CONSTRUCTION ----------------------------------------------------- */
+/* --- CONSTRUCTION ----------------------------------------------------- */
+VisualPointProjecter::VisualPointProjecter(const std::string &name)
+ : Entity(name)
+
+ ,
+ CONSTRUCT_SIGNAL_IN(point3D, dynamicgraph::Vector),
+ CONSTRUCT_SIGNAL_IN(transfo, MatrixHomogeneous)
+
+ ,
+ CONSTRUCT_SIGNAL_OUT(point3Dgaze, dynamicgraph::Vector,
+ m_point3DSIN << m_transfoSIN),
+ CONSTRUCT_SIGNAL_OUT(depth, double, m_point3DgazeSOUT),
+ CONSTRUCT_SIGNAL_OUT(point2D, dynamicgraph::Vector,
+ m_point3DgazeSOUT << m_depthSOUT) {
+ Entity::signalRegistration(m_point3DSIN);
+ Entity::signalRegistration(m_transfoSIN);
+ Entity::signalRegistration(m_point3DgazeSOUT);
+ Entity::signalRegistration(m_point2DSOUT);
+ Entity::signalRegistration(m_depthSOUT);
+}
+
+/* --- SIGNALS ---------------------------------------------------------- */
+/* --- SIGNALS ---------------------------------------------------------- */
+/* --- SIGNALS ---------------------------------------------------------- */
+
+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;
+ Mi = M.inverse(Eigen::Affine);
+ p3g = Mi.matrix() * p3;
+ return p3g;
+}
+
+dynamicgraph::Vector &
+VisualPointProjecter::point2DSOUT_function(dynamicgraph::Vector &p2, int iter) {
+ sotDEBUGIN(15);
+
+ const dynamicgraph::Vector &p3 = m_point3DgazeSOUT(iter);
+ const double &z = m_depthSOUT(iter);
+ assert(z > 0);
+
+ p2.resize(2);
+ p2(0) = p3(0) / z;
+ p2(1) = p3(1) / z;
+
+ sotDEBUGOUT(15);
+ return p2;
+}
+
+double &VisualPointProjecter::depthSOUT_function(double &z, int iter) {
+ const dynamicgraph::Vector &p3 = m_point3DgazeSOUT(iter);
+ assert(p3.size() == 3);
+ z = p3(2);
+ return z;
+}
+
+/* --- ENTITY ----------------------------------------------------------- */
+/* --- ENTITY ----------------------------------------------------------- */
+/* --- ENTITY ----------------------------------------------------------- */
+
+void VisualPointProjecter::display(std::ostream &os) const {
+ os << "VisualPointProjecter " << getName();
+}
+
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/filters/causal-filter.cpp b/src/filters/causal-filter.cpp
index d8a4d204..4d0d2c4d 100644
--- a/src/filters/causal-filter.cpp
+++ b/src/filters/causal-filter.cpp
@@ -18,109 +18,106 @@
#include <sot/core/causal-filter.hh>
-
using namespace dynamicgraph::sot;
/*
Filter data with an IIR or FIR filter.
-Filter a data sequence, x, using a digital filter. The filter is a direct form II transposed implementation of the standard difference equation.
-This means that the filter implements:
+Filter a data sequence, x, using a digital filter. The filter is a direct form
+II transposed implementation of the standard difference equation. This means
+that the filter implements:
a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
- a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)]
-where m is the degree of the numerator, n is the degree of the denominator, and N is the sample number
+where m is the degree of the numerator, n is the degree of the denominator, and
+N is the sample number
*/
-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_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_input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size()))
- , m_output_buffer(Eigen::MatrixXd::Zero(xSize, filter_denominator.size()-1))
-{
- assert(timestep>0.0 && "Timestep should be > 0");
+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_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_input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size())),
+ m_output_buffer(
+ Eigen::MatrixXd::Zero(xSize, filter_denominator.size() - 1)) {
+ assert(timestep > 0.0 && "Timestep should be > 0");
assert(m_filter_numerator.size() == m_filter_order_m);
assert(m_filter_denominator.size() == m_filter_order_n);
}
-
-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++)
+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_n-1; i++)
- m_output_buffer.col(i) = base_x*m_filter_numerator.sum()/
- m_filter_denominator.sum();
+ 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();
m_first_sample = false;
}
m_input_buffer.col(m_pt_numerator) = base_x;
-
+
Eigen::VectorXd b(m_filter_order_m);
- Eigen::VectorXd a(m_filter_order_n-1);
- b.head(m_pt_numerator+1) = m_filter_numerator.head(m_pt_numerator+1).reverse();
- b.tail(m_filter_order_m-m_pt_numerator-1) =
- m_filter_numerator.tail(m_filter_order_m-m_pt_numerator-1).reverse();
-
- a.head(m_pt_denominator+1) =
- m_filter_denominator.segment(1, m_pt_denominator+1).reverse();
- a.tail(m_filter_order_n-m_pt_denominator-2) =
- m_filter_denominator.tail(m_filter_order_n-m_pt_denominator-2).reverse();
+ Eigen::VectorXd a(m_filter_order_n - 1);
+ b.head(m_pt_numerator + 1) =
+ m_filter_numerator.head(m_pt_numerator + 1).reverse();
+ b.tail(m_filter_order_m - m_pt_numerator - 1) =
+ m_filter_numerator.tail(m_filter_order_m - m_pt_numerator - 1).reverse();
+
+ a.head(m_pt_denominator + 1) =
+ m_filter_denominator.segment(1, m_pt_denominator + 1).reverse();
+ a.tail(m_filter_order_n - m_pt_denominator - 2) =
+ m_filter_denominator.tail(m_filter_order_n - m_pt_denominator - 2)
+ .reverse();
x_output_dx_ddx.head(m_x_size) =
- (m_input_buffer*b-m_output_buffer*a)/m_filter_denominator[0];
-
- //Finite Difference
- Eigen::VectorXd::Index m_pt_denominator_prev = (m_pt_denominator == 0) ?
- m_filter_order_n-2 : m_pt_denominator-1;
- x_output_dx_ddx.segment(m_x_size,m_x_size) =
- (x_output_dx_ddx.head(m_x_size)-m_output_buffer.col(m_pt_denominator))/m_dt;
+ (m_input_buffer * b - m_output_buffer * a) / m_filter_denominator[0];
+
+ // Finite Difference
+ Eigen::VectorXd::Index m_pt_denominator_prev =
+ (m_pt_denominator == 0) ? m_filter_order_n - 2 : m_pt_denominator - 1;
+ x_output_dx_ddx.segment(m_x_size, m_x_size) =
+ (x_output_dx_ddx.head(m_x_size) - m_output_buffer.col(m_pt_denominator)) /
+ m_dt;
x_output_dx_ddx.tail(m_x_size) =
- (x_output_dx_ddx.head(m_x_size)-2*m_output_buffer.col(m_pt_denominator)
- +m_output_buffer.col(m_pt_denominator_prev))/m_dt/m_dt;
-
- m_pt_numerator = (m_pt_numerator+1) < m_filter_order_m ? (m_pt_numerator+1) : 0;
- m_pt_denominator = (m_pt_denominator+1) < m_filter_order_n-1 ?
- (m_pt_denominator+1) : 0;
+ (x_output_dx_ddx.head(m_x_size) -
+ 2 * m_output_buffer.col(m_pt_denominator) +
+ m_output_buffer.col(m_pt_denominator_prev)) /
+ m_dt / m_dt;
+
+ m_pt_numerator =
+ (m_pt_numerator + 1) < m_filter_order_m ? (m_pt_numerator + 1) : 0;
+ m_pt_denominator = (m_pt_denominator + 1) < m_filter_order_n - 1
+ ? (m_pt_denominator + 1)
+ : 0;
m_output_buffer.col(m_pt_denominator) = x_output_dx_ddx.head(m_x_size);
return;
}
-
-
-void CausalFilter::switch_filter(const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator)
-{
+void CausalFilter::switch_filter(const Eigen::VectorXd &filter_numerator,
+ const Eigen::VectorXd &filter_denominator) {
Eigen::VectorXd::Index filter_order_m = filter_numerator.size();
Eigen::VectorXd::Index filter_order_n = filter_denominator.size();
Eigen::VectorXd current_x(m_input_buffer.col(m_pt_numerator));
m_input_buffer.resize(Eigen::NoChange, filter_order_m);
- m_output_buffer.resize(Eigen::NoChange, filter_order_n-1);
+ m_output_buffer.resize(Eigen::NoChange, filter_order_n - 1);
- for(int i=0;i<filter_order_m; i++)
+ 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) = current_x*
- filter_numerator.sum()/filter_denominator.sum();
-
+ for (int i = 0; i < filter_order_n - 1; i++)
+ m_output_buffer.col(i) =
+ current_x * filter_numerator.sum() / filter_denominator.sum();
m_filter_order_m = filter_order_m;
m_filter_numerator.resize(filter_order_m);
diff --git a/src/filters/filter-differentiator.cpp b/src/filters/filter-differentiator.cpp
index c8d08b8e..f30a7e3f 100644
--- a/src/filters/filter-differentiator.cpp
+++ b/src/filters/filter-differentiator.cpp
@@ -16,193 +16,159 @@
#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 <sot/core/filter-differentiator.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
#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>
#include <Eigen/Dense>
-namespace dynamicgraph
-{
- namespace sot
- {
+namespace dynamicgraph {
+namespace sot {
#define ALL_INPUT_SIGNALS m_xSIN
-#define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT
-
- namespace dynamicgraph = ::dynamicgraph;
- using namespace dynamicgraph;
- using namespace dynamicgraph::command;
- using namespace Eigen;
-
- /// Define EntityClassName here rather than in the header file
- /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef FilterDifferentiator EntityClassName;
-
- /* --- DG FACTORY --------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN
- (FilterDifferentiator,"FilterDifferentiator");
-
- /* --- CONSTRUCTION ------------------------------------------------- */
- /* --- CONSTRUCTION ------------------------------------------------- */
- /* --- CONSTRUCTION ------------------------------------------------- */
- FilterDifferentiator::
- FilterDifferentiator( const std::string & name )
- : 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)
- ,CONSTRUCT_SIGNAL_INNER
- (x_dx_ddx,
- dynamicgraph::Vector, m_xSIN)
- {
- Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
-
- /* Commands. */
- addCommand("getTimestep",
- makeDirectGetter
- (*this,&m_dt,
- docDirectGetter("Control timestep [s ]","double")));
- addCommand("getSize",
- makeDirectGetter
- (*this,&m_x_size,
- docDirectGetter("Size of the x signal","int")));
- addCommand("init",
- makeCommandVoid4
- (*this, &FilterDifferentiator::init,
- docCommandVoid4
- ("Initialize the filter.",
- "Control timestep [s].",
- "Size of the input signal x",
- "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")));
- }
-
-
- /* --- COMMANDS ------------------------------------------------------ */
- /* --- COMMANDS ------------------------------------------------------ */
- /* --- COMMANDS ------------------------------------------------------ */
- void FilterDifferentiator::
- init(const double ×tep,
- const int& xSize,
- const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator)
- {
- m_x_size = xSize;
- m_dt = timestep;
- m_filter = new CausalFilter(timestep, xSize,
- filter_numerator, filter_denominator);
-
- LOG("Filtering started with "<<
- "Numerator "<< filter_numerator<<std::endl<<
- "Denominator"<<filter_denominator<<std::endl);
- return;
- }
-
- void FilterDifferentiator::
- switch_filter
- (const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator)
- {
- LOG("Filter switched with "<<
- "Numerator "<< filter_numerator<<std::endl<<
- "Denominator"<<filter_denominator<<std::endl<<
- "at time"<<m_xSIN.getTime());
- m_filter->switch_filter(filter_numerator, filter_denominator);
- }
-
-
- /* --- SIGNALS ------------------------------------------------------ */
- /* --- SIGNALS ------------------------------------------------------ */
- /* --- SIGNALS ------------------------------------------------------ */
-
- 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);
- // read encoders
- const dynamicgraph::Vector& base_x = m_xSIN(iter);
- assert(base_x.size() == m_x_size);
- m_filter->get_x_dx_ddx(base_x, s);
- return s;
- }
-
-
- /// *************************************************************** ///
- /// The following signals depend only on other inner signals, so they
- /// just need to copy the interested part of the inner signal
- /// they depend on.
- /// *************************************************************** ///
-
-
- 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);
- s = x_dx_ddx.head(m_x_size);
- return s;
- }
-
- 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);
- s = x_dx_ddx.segment(m_x_size, m_x_size);
- return s;
- }
-
- 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);
- s = x_dx_ddx.tail(m_x_size);
- return s;
- }
-
- void FilterDifferentiator::display( std::ostream& os ) const
- {
- os << "FilterDifferentiator "<<getName()<<":\n";
- try
- {
- getProfiler().report_all(3, os);
- }
- catch (ExceptionSignal e) {}
- }
-
- } // namespace sot
+#define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT
+
+namespace dynamicgraph = ::dynamicgraph;
+using namespace dynamicgraph;
+using namespace dynamicgraph::command;
+using namespace Eigen;
+
+/// Define EntityClassName here rather than in the header file
+/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+typedef FilterDifferentiator EntityClassName;
+
+/* --- DG FACTORY --------------------------------------------------- */
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FilterDifferentiator,
+ "FilterDifferentiator");
+
+/* --- CONSTRUCTION ------------------------------------------------- */
+/* --- CONSTRUCTION ------------------------------------------------- */
+/* --- CONSTRUCTION ------------------------------------------------- */
+FilterDifferentiator::FilterDifferentiator(const std::string &name)
+ : 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),
+ CONSTRUCT_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector, m_xSIN) {
+ Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
+
+ /* Commands. */
+ addCommand(
+ "getTimestep",
+ makeDirectGetter(*this, &m_dt,
+ docDirectGetter("Control timestep [s ]", "double")));
+ addCommand("getSize",
+ makeDirectGetter(*this, &m_x_size,
+ docDirectGetter("Size of the x signal", "int")));
+ addCommand("init",
+ makeCommandVoid4(*this, &FilterDifferentiator::init,
+ docCommandVoid4("Initialize the filter.",
+ "Control timestep [s].",
+ "Size of the input signal x",
+ "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")));
+}
+
+/* --- COMMANDS ------------------------------------------------------ */
+/* --- COMMANDS ------------------------------------------------------ */
+/* --- COMMANDS ------------------------------------------------------ */
+void FilterDifferentiator::init(const double ×tep, const int &xSize,
+ const Eigen::VectorXd &filter_numerator,
+ const Eigen::VectorXd &filter_denominator) {
+ m_x_size = xSize;
+ m_dt = timestep;
+ m_filter =
+ new CausalFilter(timestep, xSize, filter_numerator, filter_denominator);
+
+ LOG("Filtering started with "
+ << "Numerator " << filter_numerator << std::endl
+ << "Denominator" << filter_denominator << std::endl);
+ return;
+}
+
+void FilterDifferentiator::switch_filter(
+ const Eigen::VectorXd &filter_numerator,
+ const Eigen::VectorXd &filter_denominator) {
+ LOG("Filter switched with "
+ << "Numerator " << filter_numerator << std::endl
+ << "Denominator" << filter_denominator << std::endl
+ << "at time" << m_xSIN.getTime());
+ m_filter->switch_filter(filter_numerator, filter_denominator);
+}
+
+/* --- SIGNALS ------------------------------------------------------ */
+/* --- SIGNALS ------------------------------------------------------ */
+/* --- SIGNALS ------------------------------------------------------ */
+
+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);
+ // read encoders
+ const dynamicgraph::Vector &base_x = m_xSIN(iter);
+ assert(base_x.size() == m_x_size);
+ m_filter->get_x_dx_ddx(base_x, s);
+ return s;
+}
+
+/// *************************************************************** ///
+/// The following signals depend only on other inner signals, so they
+/// just need to copy the interested part of the inner signal
+/// they depend on.
+/// *************************************************************** ///
+
+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);
+ s = x_dx_ddx.head(m_x_size);
+ return s;
+}
+
+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);
+ s = x_dx_ddx.segment(m_x_size, m_x_size);
+ return s;
+}
+
+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);
+ s = x_dx_ddx.tail(m_x_size);
+ return s;
+}
+
+void FilterDifferentiator::display(std::ostream &os) const {
+ os << "FilterDifferentiator " << getName() << ":\n";
+ try {
+ getProfiler().report_all(3, os);
+ } catch (ExceptionSignal e) {
+ }
+}
+
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/filters/madgwickahrs.cpp b/src/filters/madgwickahrs.cpp
index a827716d..e21be756 100644
--- a/src/filters/madgwickahrs.cpp
+++ b/src/filters/madgwickahrs.cpp
@@ -10,267 +10,221 @@
//
//=========================================================================
-
-
-#include <sot/core/madgwickahrs.hh>
-#include <sot/core/debug.hh>
#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
-{
- namespace sot
+namespace dynamicgraph {
+namespace sot {
+namespace dg = ::dynamicgraph;
+using namespace dg;
+using namespace dg::command;
+using namespace std;
+
+typedef Vector6d Vector6;
+
+#define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation"
+
+#define INPUT_SIGNALS m_accelerometerSIN << m_gyroscopeSIN
+#define OUTPUT_SIGNALS m_imu_quatSOUT
+
+/// Define EntityClassName here rather than in the header file
+/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+typedef MadgwickAHRS EntityClassName;
+
+/* --- DG FACTORY ---------------------------------------------------- */
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS, "MadgwickAHRS");
+
+/* ------------------------------------------------------------------- */
+/* --- CONSTRUCTION -------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+MadgwickAHRS::MadgwickAHRS(const std::string &name)
+ : 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) {
+ Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
+
+ /* Commands. */
+ addCommand("init",
+ makeCommandVoid1(*this, &MadgwickAHRS::init,
+ docCommandVoid1("Initialize the entity.",
+ "Timestep in seconds (double)")));
+ addCommand("getBeta",
+ makeDirectGetter(*this, &m_beta,
+ docDirectGetter("Beta parameter", "double")));
+ addCommand("setBeta",
+ makeCommandVoid1(
+ *this, &MadgwickAHRS::set_beta,
+ docCommandVoid1("Set the filter parameter beta", "double")));
+ addCommand("set_imu_quat",
+ makeCommandVoid1(
+ *this, &MadgwickAHRS::set_imu_quat,
+ docCommandVoid1("Set the quaternion as [w,x,y,z]", "vector")));
+}
+
+void MadgwickAHRS::init(const double &dt) {
+ if (dt <= 0.0)
+ return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+ m_sampleFreq = 1.0 / dt;
+ m_initSucceeded = true;
+}
+
+void MadgwickAHRS::set_beta(const double &beta) {
+ if (beta < 0.0 || beta > 1.0)
+ return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
+ m_beta = beta;
+}
+
+void MadgwickAHRS::set_imu_quat(const dynamicgraph::Vector &imu_quat) {
+ assert(imu_quat.size() == 4);
+ m_q0 = imu_quat[0];
+ m_q1 = imu_quat[1];
+ m_q2 = imu_quat[2];
+ m_q3 = imu_quat[3];
+}
+
+/* ------------------------------------------------------------------- */
+/* --- SIGNALS ------------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+
+DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG(
+ "Cannot compute signal imu_quat before initialization!");
+ return s;
+ }
+ const dynamicgraph::Vector &accelerometer = m_accelerometerSIN(iter);
+ const dynamicgraph::Vector &gyroscope = m_gyroscopeSIN(iter);
+
+ getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
{
- namespace dg = ::dynamicgraph;
- using namespace dg;
- using namespace dg::command;
- using namespace std;
-
- typedef Vector6d Vector6;
-
-#define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation"
-
-#define INPUT_SIGNALS m_accelerometerSIN << m_gyroscopeSIN
-#define OUTPUT_SIGNALS m_imu_quatSOUT
-
- /// Define EntityClassName here rather than in the header file
- /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef MadgwickAHRS EntityClassName;
-
- /* --- DG FACTORY ---------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS,
- "MadgwickAHRS");
-
- /* ------------------------------------------------------------------- */
- /* --- CONSTRUCTION -------------------------------------------------- */
- /* ------------------------------------------------------------------- */
- MadgwickAHRS::MadgwickAHRS(const std::string& name)
- : 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)
- {
- Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
-
- /* Commands. */
- addCommand
- ("init",
- makeCommandVoid1
- (*this, &MadgwickAHRS::init,
- docCommandVoid1
- ("Initialize the entity.",
- "Timestep in seconds (double)")));
- addCommand
- ("getBeta",
- makeDirectGetter
- (*this, &m_beta,
- docDirectGetter("Beta parameter", "double")));
- addCommand
- ("setBeta",
- makeCommandVoid1
- (*this, &MadgwickAHRS::set_beta,
- docCommandVoid1("Set the filter parameter beta", "double")));
- addCommand
- ("set_imu_quat",
- makeCommandVoid1
- (*this, &MadgwickAHRS::set_imu_quat,
- docCommandVoid1("Set the quaternion as [w,x,y,z]", "vector")));
- }
-
- void MadgwickAHRS::init(const double& dt)
- {
- if(dt<=0.0)
- return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
- m_sampleFreq=1.0/dt;
- m_initSucceeded = true;
- }
-
- void MadgwickAHRS::set_beta(const double& beta)
- {
- if(beta<0.0 || beta>1.0)
- return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
- m_beta = beta;
- }
-
- void MadgwickAHRS::set_imu_quat(const dynamicgraph::Vector & imu_quat)
- {
- assert(imu_quat.size()==4);
- m_q0 = imu_quat[0];
- m_q1 = imu_quat[1];
- m_q2 = imu_quat[2];
- m_q3 = imu_quat[3];
- }
-
- /* ------------------------------------------------------------------- */
- /* --- SIGNALS ------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
- DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG
- ("Cannot compute signal imu_quat before initialization!");
- return s;
- }
- const dynamicgraph::Vector& accelerometer = m_accelerometerSIN(iter);
- const dynamicgraph::Vector& gyroscope = m_gyroscopeSIN(iter);
-
- getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
- {
- // 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);
- s(0) = m_q0;
- s(1) = m_q1;
- s(2) = m_q2;
- s(3) = m_q3;
- }
- getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION);
-
- return s;
+ // 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);
+ s(0) = m_q0;
+ s(1) = m_q1;
+ s(2) = m_q2;
+ s(3) = m_q3;
+ }
+ getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION);
+
+ return s;
+}
+
+/* --- COMMANDS ------------------------------------------------------ */
+
+/* ------------------------------------------------------------------- */
+// ************************ PROTECTED MEMBER METHODS ********************
+/* ------------------------------------------------------------------- */
+
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+double MadgwickAHRS::invSqrt(double x) {
+ /*
+ float halfx = 0.5f * x;
+ float y = x;
+ long i = *(long*)&y;
+ i = 0x5f3759df - (i>>1);
+ y = *(float*)&i;
+ y = y * (1.5f - (halfx * y * y));
+ return y;*/
+ return (1.0 / sqrt(x)); // we're not in the 70's
+}
+
+// IMU algorithm update
+void MadgwickAHRS::madgwickAHRSupdateIMU(double gx, double gy, double gz,
+ double ax, double ay, double az) {
+ double recipNorm;
+ double s0, s1, s2, s3;
+ double qDot1, qDot2, qDot3, qDot4;
+ double o2q0, o2q1, o2q2, o2q3, o4q0, o4q1, o4q2, o8q1, o8q2;
+ double q0q0, q1q1, q2q2, q3q3;
+
+ // Rate of change of quaternion from gyroscope
+ qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz);
+ qDot2 = 0.5 * (m_q0 * gx + m_q2 * gz - m_q3 * gy);
+ qDot3 = 0.5 * (m_q0 * gy - m_q1 * gz + m_q3 * gx);
+ qDot4 = 0.5 * (m_q0 * gz + m_q1 * gy - m_q2 * gx);
+
+ // Compute feedback only if accelerometer measurement valid
+ // (avoids NaN in accelerometer normalisation)
+ if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
+ // Normalise accelerometer measurement
+ recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ o2q0 = 2.0 * m_q0;
+ o2q1 = 2.0 * m_q1;
+ o2q2 = 2.0 * m_q2;
+ o2q3 = 2.0 * m_q3;
+ o4q0 = 4.0 * m_q0;
+ o4q1 = 4.0 * m_q1;
+ o4q2 = 4.0 * m_q2;
+ o8q1 = 8.0 * m_q1;
+ o8q2 = 8.0 * m_q2;
+ q0q0 = m_q0 * m_q0;
+ q1q1 = m_q1 * m_q1;
+ q2q2 = m_q2 * m_q2;
+ q3q3 = m_q3 * m_q3;
+
+ // Gradient decent algorithm corrective step
+ s0 = o4q0 * q2q2 + o2q2 * ax + o4q0 * q1q1 - o2q1 * ay;
+ s1 = o4q1 * q3q3 - o2q3 * ax + 4.0 * q0q0 * m_q1 - o2q0 * ay - o4q1 +
+ o8q1 * q1q1 + o8q1 * q2q2 + o4q1 * az;
+ s2 = 4.0 * q0q0 * m_q2 + o2q0 * ax + o4q2 * q3q3 - o2q3 * ay - o4q2 +
+ o8q2 * q1q1 + o8q2 * q2q2 + o4q2 * az;
+ s3 = 4.0 * q1q1 * m_q3 - o2q1 * ax + 4.0 * q2q2 * m_q3 - o2q2 * ay;
+ if (!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0))) {
+ // normalise step magnitude
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+ s0 *= recipNorm;
+ s1 *= recipNorm;
+ s2 *= recipNorm;
+ s3 *= recipNorm;
+
+ // Apply feedback step
+ qDot1 -= m_beta * s0;
+ qDot2 -= m_beta * s1;
+ qDot3 -= m_beta * s2;
+ qDot4 -= m_beta * s3;
}
-
-
- /* --- COMMANDS ------------------------------------------------------ */
-
- /* ------------------------------------------------------------------- */
- // ************************ PROTECTED MEMBER METHODS ********************
- /* ------------------------------------------------------------------- */
-
- // Fast inverse square-root
- // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
- double MadgwickAHRS::invSqrt(double x)
- {
- /*
- float halfx = 0.5f * x;
- float y = x;
- long i = *(long*)&y;
- i = 0x5f3759df - (i>>1);
- y = *(float*)&i;
- y = y * (1.5f - (halfx * y * y));
- return y;*/
- return (1.0/sqrt(x)); //we're not in the 70's
- }
-
- // IMU algorithm update
- void MadgwickAHRS::
- madgwickAHRSupdateIMU
- (double gx, double gy, double gz, double ax, double ay, double az)
- {
- double recipNorm;
- double s0, s1, s2, s3;
- double qDot1, qDot2, qDot3, qDot4;
- double o2q0, o2q1, o2q2, o2q3, o4q0, o4q1, o4q2 ,o8q1, o8q2;
- double q0q0, q1q1, q2q2, q3q3;
-
- // Rate of change of quaternion from gyroscope
- qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz);
- qDot2 = 0.5 * ( m_q0 * gx + m_q2 * gz - m_q3 * gy);
- qDot3 = 0.5 * ( m_q0 * gy - m_q1 * gz + m_q3 * gx);
- qDot4 = 0.5 * ( m_q0 * gz + m_q1 * gy - m_q2 * gx);
-
- // Compute feedback only if accelerometer measurement valid
- // (avoids NaN in accelerometer normalisation)
- if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0)))
- {
- // Normalise accelerometer measurement
- recipNorm = invSqrt(ax * ax + ay * ay + az * az);
- ax *= recipNorm;
- ay *= recipNorm;
- az *= recipNorm;
-
- // Auxiliary variables to avoid repeated arithmetic
- o2q0 = 2.0 * m_q0;
- o2q1 = 2.0 * m_q1;
- o2q2 = 2.0 * m_q2;
- o2q3 = 2.0 * m_q3;
- o4q0 = 4.0 * m_q0;
- o4q1 = 4.0 * m_q1;
- o4q2 = 4.0 * m_q2;
- o8q1 = 8.0 * m_q1;
- o8q2 = 8.0 * m_q2;
- q0q0 = m_q0 * m_q0;
- q1q1 = m_q1 * m_q1;
- q2q2 = m_q2 * m_q2;
- q3q3 = m_q3 * m_q3;
-
- // Gradient decent algorithm corrective step
- s0 = o4q0 * q2q2 + o2q2 * ax + o4q0 * q1q1 - o2q1 * ay;
- s1 = o4q1 * q3q3 - o2q3 * ax + 4.0 * q0q0 * m_q1 - o2q0 * ay -
- o4q1 + o8q1 * q1q1 + o8q1 * q2q2 + o4q1 * az;
- s2 = 4.0 * q0q0 * m_q2 + o2q0 * ax + o4q2 * q3q3 - o2q3 * ay -
- o4q2 + o8q2 * q1q1 + o8q2 * q2q2 + o4q2 * az;
- s3 = 4.0 * q1q1 * m_q3 - o2q1 * ax + 4.0 * q2q2 * m_q3 - o2q2 * ay;
- if(!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0)))
- {
- // normalise step magnitude
- recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
- s0 *= recipNorm;
- s1 *= recipNorm;
- s2 *= recipNorm;
- s3 *= recipNorm;
-
- // Apply feedback step
- qDot1 -= m_beta * s0;
- qDot2 -= m_beta * s1;
- qDot3 -= m_beta * s2;
- qDot4 -= m_beta * s3;
- }
- }
-
- // Integrate rate of change of quaternion to yield quaternion
- m_q0 += qDot1 * (1.0 / m_sampleFreq);
- m_q1 += qDot2 * (1.0 / m_sampleFreq);
- m_q2 += qDot3 * (1.0 / m_sampleFreq);
- m_q3 += qDot4 * (1.0 / m_sampleFreq);
-
- // Normalise quaternion
- recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 +
- m_q2 * m_q2 + m_q3 * m_q3);
- m_q0 *= recipNorm;
- m_q1 *= recipNorm;
- m_q2 *= recipNorm;
- m_q3 *= recipNorm;
- }
-
-
- /* ------------------------------------------------------------------- */
- /* --- ENTITY -------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
- void MadgwickAHRS::display(std::ostream& os) const
- {
- os << "MadgwickAHRS "<<getName();
- try
- {
- getProfiler().report_all(3, os);
- }
- catch (ExceptionSignal e) {}
- }
- } // namespace sot
+ }
+
+ // Integrate rate of change of quaternion to yield quaternion
+ m_q0 += qDot1 * (1.0 / m_sampleFreq);
+ m_q1 += qDot2 * (1.0 / m_sampleFreq);
+ m_q2 += qDot3 * (1.0 / m_sampleFreq);
+ m_q3 += qDot4 * (1.0 / m_sampleFreq);
+
+ // Normalise quaternion
+ recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 + m_q2 * m_q2 + m_q3 * m_q3);
+ m_q0 *= recipNorm;
+ m_q1 *= recipNorm;
+ m_q2 *= recipNorm;
+ m_q3 *= recipNorm;
+}
+
+/* ------------------------------------------------------------------- */
+/* --- ENTITY -------------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+
+void MadgwickAHRS::display(std::ostream &os) const {
+ os << "MadgwickAHRS " << getName();
+ try {
+ getProfiler().report_all(3, os);
+ } catch (ExceptionSignal e) {
+ }
+}
+} // namespace sot
} // namespace dynamicgraph
-
-
-
-
-
-
-
diff --git a/src/math/op-point-modifier.cpp b/src/math/op-point-modifier.cpp
index 7dab6705..10b3b8d3 100644
--- a/src/math/op-point-modifier.cpp
+++ b/src/math/op-point-modifier.cpp
@@ -7,138 +7,127 @@
*
*/
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/all-signals.h>
+#include <dynamic-graph/factory.h>
-#include <sot/core/op-point-modifier.hh>
#include <sot/core/matrix-geometry.hh>
-
+#include <sot/core/op-point-modifier.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(OpPointModifier,"OpPointModifier");
-
+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")
- ,jacobianSOUT
- ( boost::bind(&OpPointModifier::jacobianSOUT_function,
- this,_1,_2),
- jacobianSIN,
- "OpPointModifior("+name+")::output(matrix)::jacobian" )
- ,positionSOUT
- ( boost::bind(&OpPointModifier::positionSOUT_function,this,_1,_2),
- positionSIN,
- "OpPointModifior("+name+")::output(matrixhomo)::position" )
- ,transformation()
- ,isEndEffector(true)
-{
+OpPointModifier::OpPointModifier(const std::string &name)
+ : 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,
+ "OpPointModifior(" + name + ")::output(matrix)::jacobian"),
+ positionSOUT(
+ boost::bind(&OpPointModifier::positionSOUT_function, this, _1, _2),
+ positionSIN,
+ "OpPointModifior(" + name + ")::output(matrixhomo)::position"),
+ transformation(), isEndEffector(true) {
sotDEBUGIN(15);
- signalRegistration( jacobianSIN<<positionSIN<<jacobianSOUT<<positionSOUT );
+ signalRegistration(jacobianSIN << positionSIN << jacobianSOUT
+ << positionSOUT);
{
using namespace dynamicgraph::command;
- addCommand
- ("getTransformation",
- makeDirectGetter(*this,&transformation.matrix(),
- docDirectGetter("transformation","matrix 4x4 homo")));
- addCommand
- ("setTransformation",
- makeDirectSetter(*this, &transformation.matrix(),
- docDirectSetter("dimension","matrix 4x4 homo")));
- addCommand
- ("getEndEffector",
- makeDirectGetter(*this,&isEndEffector,
- docDirectGetter("end effector mode","bool")));
- addCommand
- ("setEndEffector",
- makeDirectSetter(*this, &isEndEffector,
- docDirectSetter("end effector mode","bool")));
+ addCommand(
+ "getTransformation",
+ makeDirectGetter(*this, &transformation.matrix(),
+ docDirectGetter("transformation", "matrix 4x4 homo")));
+ addCommand(
+ "setTransformation",
+ makeDirectSetter(*this, &transformation.matrix(),
+ docDirectSetter("dimension", "matrix 4x4 homo")));
+ addCommand("getEndEffector",
+ makeDirectGetter(*this, &isEndEffector,
+ docDirectGetter("end effector mode", "bool")));
+ addCommand("setEndEffector",
+ makeDirectSetter(*this, &isEndEffector,
+ docDirectSetter("end effector mode", "bool")));
}
sotDEBUGOUT(15);
}
-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
- return res;
- }
- else
- {
- /* Consider that the jacobian of point A in frame A is given: J = aJa
- * and that homogenous transformation from A to B is given aMb in getTransfo()
- * and homo transfo from 0 to A is given oMa in positionSIN.
- * Then return oJb, the jacobian of point B expressed in frame O:
- * oJb = ( oRa 0 ; 0 oRa ) * bVa * aJa
- * = [ I skew(oAB); 0 I ] * oJa
- * with oAB = oRb bAB = oRb (-bRa aAB ) = -oRa aAB, and aAB = translation(aMb).
- */
-
- const dynamicgraph::Matrix& oJa = jacobianSIN( iter );
- const MatrixHomogeneous & aMb = transformation;
- const MatrixHomogeneous & oMa = positionSIN(iter);
- MatrixRotation oRa; oRa = oMa.linear();
- dynamicgraph::Vector aAB(3); aAB = aMb.translation();
- dynamicgraph::Vector oAB = oRa*aAB;
-
- const dynamicgraph::Matrix::Index nq = oJa.cols();
- res.resize( 6,oJa.cols() );
- for( int j=0;j<nq;++j )
- {
- /* This is a I*Jtrans + skew*Jrot product, unrolled by hand ... */
- res(0,j) = oJa(0,j) -oAB(1)*oJa(2+3,j) + oAB(2)*oJa(1+3,j);
- res(1,j) = oJa(1,j) -oAB(2)*oJa(0+3,j) + oAB(0)*oJa(2+3,j);
- res(2,j) = oJa(2,j) -oAB(0)*oJa(1+3,j) + oAB(1)*oJa(0+3,j);
- for( int i=0;i<3;++i )
- {
- res(i+3,j) = oJa(i+3,j);
- }
- }
- return res; // res := 0Jb
+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
+ return res;
+ } else {
+ /* Consider that the jacobian of point A in frame A is given: J = aJa
+ * and that homogenous transformation from A to B is given aMb in
+ * getTransfo() and homo transfo from 0 to A is given oMa in positionSIN.
+ * Then return oJb, the jacobian of point B expressed in frame O:
+ * oJb = ( oRa 0 ; 0 oRa ) * bVa * aJa
+ * = [ I skew(oAB); 0 I ] * oJa
+ * with oAB = oRb bAB = oRb (-bRa aAB ) = -oRa aAB, and aAB =
+ * translation(aMb).
+ */
+
+ const dynamicgraph::Matrix &oJa = jacobianSIN(iter);
+ const MatrixHomogeneous &aMb = transformation;
+ const MatrixHomogeneous &oMa = positionSIN(iter);
+ MatrixRotation oRa;
+ oRa = oMa.linear();
+ dynamicgraph::Vector aAB(3);
+ aAB = aMb.translation();
+ dynamicgraph::Vector oAB = oRa * aAB;
+
+ const dynamicgraph::Matrix::Index nq = oJa.cols();
+ res.resize(6, oJa.cols());
+ for (int j = 0; j < nq; ++j) {
+ /* This is a I*Jtrans + skew*Jrot product, unrolled by hand ... */
+ res(0, j) = oJa(0, j) - oAB(1) * oJa(2 + 3, j) + oAB(2) * oJa(1 + 3, j);
+ res(1, j) = oJa(1, j) - oAB(2) * oJa(0 + 3, j) + oAB(0) * oJa(2 + 3, j);
+ res(2, j) = oJa(2, j) - oAB(0) * oJa(1 + 3, j) + oAB(1) * oJa(0 + 3, j);
+ for (int i = 0; i < 3; ++i) {
+ res(i + 3, j) = oJa(i + 3, j);
+ }
}
+ 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;
- const MatrixHomogeneous& position = positionSIN( iter );
- res = position*transformation;
+ << positionSOUT.getTime() << endl;
+ const MatrixHomogeneous &position = positionSIN(iter);
+ res = position * transformation;
sotDEBUGOUT(15);
return res;
}
-void
-OpPointModifier::setTransformation( const Eigen::Matrix4d& tr )
-{ transformation.matrix() = tr; }
-const Eigen::Matrix4d&
-OpPointModifier::getTransformation( void )
-{ return transformation.matrix(); }
-
+void OpPointModifier::setTransformation(const Eigen::Matrix4d &tr) {
+ transformation.matrix() = tr;
+}
+const Eigen::Matrix4d &OpPointModifier::getTransformation(void) {
+ return transformation.matrix();
+}
/* The following function needs an access to a specific signal via
* the pool, using the signal path <entity.signal>. this functionnality
@@ -147,11 +136,10 @@ OpPointModifier::getTransformation( void )
* the <setTransformation> mthod, bound in python.
*/
#include <dynamic-graph/pool.h>
-void
-OpPointModifier::setTransformationBySignalName( std::istringstream& cmdArgs )
-{
- Signal< Eigen::Matrix4d,int > &sig
- = dynamic_cast< Signal< Eigen::Matrix4d,int >& >
- (PoolStorage::getInstance()->getSignal( cmdArgs ));
+void OpPointModifier::setTransformationBySignalName(
+ std::istringstream &cmdArgs) {
+ Signal<Eigen::Matrix4d, int> &sig =
+ dynamic_cast<Signal<Eigen::Matrix4d, int> &>(
+ PoolStorage::getInstance()->getSignal(cmdArgs));
setTransformation(sig.accessCopy());
}
diff --git a/src/math/vector-quaternion.cpp b/src/math/vector-quaternion.cpp
index 5fbbb1e9..ead76d20 100644
--- a/src/math/vector-quaternion.cpp
+++ b/src/math/vector-quaternion.cpp
@@ -7,8 +7,8 @@
*
*/
-#include <sot/core/matrix-geometry.hh>
#include <sot/core/debug.hh>
+#include <sot/core/matrix-geometry.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -17,105 +17,88 @@ static const double ANGLE_MINIMUM = 0.0001;
static const double SINC_MINIMUM = 1e-8;
static const double COSC_MINIMUM = 2.5e-4;
-VectorRotation& VectorQuaternion::
-fromMatrix( const MatrixRotation& rot )
-{
- sotDEBUGIN(15) ;
+VectorRotation &VectorQuaternion::fromMatrix(const MatrixRotation &rot) {
+ sotDEBUGIN(15);
- const dynamicgraph::Matrix& rotmat = rot;
+ const dynamicgraph::Matrix &rotmat = rot;
- double d0 = rotmat(0,0), d1 = rotmat(1,1), d2 = rotmat(2,2);
+ double d0 = rotmat(0, 0), d1 = rotmat(1, 1), d2 = rotmat(2, 2);
// The trace determines the method of decomposition
double rr = 1.0 + d0 + d1 + d2;
- double & _x = vector(1);
- double & _y = vector(2);
- double & _z = vector(3);
- double & _r = vector(0);
-
- if (rr>0)
- {
- double s = 0.5 / sqrt(rr);
- _x = (rotmat(2,1) - rotmat(1,2)) * s;
- _y = (rotmat(0,2) - rotmat(2,0)) * s;
- _z = (rotmat(1,0) - rotmat(0,1)) * s;
- _r = 0.25 / s;
- }
- else
- {
- // Trace is less than zero, so need to determine which
- // major diagonal is largest
- if ((d0 > d1) && (d0 > d2))
- {
- double s = 0.5 / sqrt(1 + d0 - d1 - d2);
- _x = 0.5 * s;
- _y = (rotmat(0,1) + rotmat(1,0)) * s;
- _z = (rotmat(0,2) + rotmat(2,0)) * s;
- _r = (rotmat(1,2) + rotmat(2,1)) * s;
- }
- else if (d1 > d2)
- {
- double s = 0.5 / sqrt(1 + d0 - d1 - d2);
- _x = (rotmat(0,1) + rotmat(1,0)) * s;
- _y = 0.5 * s;
- _z = (rotmat(1,2) + rotmat(2,1)) * s;
- _r = (rotmat(0,2) + rotmat(2,0)) * s;
- }
- else
- {
- double s = 0.5 / sqrt(1 + d0 - d1 - d2);
- _x = (rotmat(0,2) + rotmat(2,0)) * s;
- _y = (rotmat(1,2) + rotmat(2,1)) * s;
- _z = 0.5 * s;
- _r = (rotmat(0,1) + rotmat(1,0)) * s;
- }
+ double &_x = vector(1);
+ double &_y = vector(2);
+ double &_z = vector(3);
+ double &_r = vector(0);
+
+ if (rr > 0) {
+ double s = 0.5 / sqrt(rr);
+ _x = (rotmat(2, 1) - rotmat(1, 2)) * s;
+ _y = (rotmat(0, 2) - rotmat(2, 0)) * s;
+ _z = (rotmat(1, 0) - rotmat(0, 1)) * s;
+ _r = 0.25 / s;
+ } else {
+ // Trace is less than zero, so need to determine which
+ // major diagonal is largest
+ if ((d0 > d1) && (d0 > d2)) {
+ double s = 0.5 / sqrt(1 + d0 - d1 - d2);
+ _x = 0.5 * s;
+ _y = (rotmat(0, 1) + rotmat(1, 0)) * s;
+ _z = (rotmat(0, 2) + rotmat(2, 0)) * s;
+ _r = (rotmat(1, 2) + rotmat(2, 1)) * s;
+ } else if (d1 > d2) {
+ double s = 0.5 / sqrt(1 + d0 - d1 - d2);
+ _x = (rotmat(0, 1) + rotmat(1, 0)) * s;
+ _y = 0.5 * s;
+ _z = (rotmat(1, 2) + rotmat(2, 1)) * s;
+ _r = (rotmat(0, 2) + rotmat(2, 0)) * s;
+ } else {
+ double s = 0.5 / sqrt(1 + d0 - d1 - d2);
+ _x = (rotmat(0, 2) + rotmat(2, 0)) * s;
+ _y = (rotmat(1, 2) + rotmat(2, 1)) * s;
+ _z = 0.5 * s;
+ _r = (rotmat(0, 1) + rotmat(1, 0)) * s;
}
+ }
- sotDEBUGOUT(15) ;
+ sotDEBUGOUT(15);
return *this;
}
+VectorRotation &VectorQuaternion::fromVector(const VectorUTheta &ut) {
+ sotDEBUGIN(15);
-
-VectorRotation& VectorQuaternion::
-fromVector( const VectorUTheta& ut )
-{
- sotDEBUGIN(15) ;
-
- double theta = sqrt( ut(0)*ut(0)+ut(1)*ut(1)+ut(2)*ut(2) );
+ double theta = sqrt(ut(0) * ut(0) + ut(1) * ut(1) + ut(2) * ut(2));
double si = sin(theta);
double co = cos(theta);
- vector(0)=ut(0)/si;
- vector(1)=ut(1)/si;
- vector(2)=ut(2)/si;
- vector(3)=co;
+ vector(0) = ut(0) / si;
+ vector(1) = ut(1) / si;
+ vector(2) = ut(2) / si;
+ vector(3) = co;
- sotDEBUGOUT(15) ;
+ sotDEBUGOUT(15);
return *this;
}
+MatrixRotation &VectorQuaternion::toMatrix(MatrixRotation &rot) const {
+ sotDEBUGIN(15);
-MatrixRotation& VectorQuaternion::
-toMatrix( MatrixRotation& rot ) const
-{
- sotDEBUGIN(15) ;
-
- dynamicgraph::Matrix& rotmat = rot;
+ dynamicgraph::Matrix &rotmat = rot;
- const double& _x = vector(1);
- const double& _y = vector(2);
- const double& _z = vector(3);
- const double& _r = vector(0);
+ const double &_x = vector(1);
+ const double &_y = vector(2);
+ const double &_z = vector(3);
+ const double &_r = vector(0);
double x2 = _x * _x;
double y2 = _y * _y;
double z2 = _z * _z;
double r2 = _r * _r;
- rotmat(0,0) = r2 + x2 - y2 - z2; // fill diagonal terms
- rotmat(1,1) = r2 - x2 + y2 - z2;
- rotmat(2,2) = r2 - x2 - y2 + z2;
+ rotmat(0, 0) = r2 + x2 - y2 - z2; // fill diagonal terms
+ rotmat(1, 1) = r2 - x2 + y2 - z2;
+ rotmat(2, 2) = r2 - x2 - y2 + z2;
double xy = _x * _y;
double yz = _y * _z;
@@ -124,20 +107,18 @@ toMatrix( MatrixRotation& rot ) const
double ry = _r * _y;
double rz = _r * _z;
- 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);
- rotmat(2,0) = 2 * (zx - ry);
- rotmat(2,1) = 2 * (yz + rx);
+ 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);
+ rotmat(2, 0) = 2 * (zx - ry);
+ rotmat(2, 1) = 2 * (yz + rx);
- sotDEBUGOUT(15) ;
+ sotDEBUGOUT(15);
return rot;
}
-VectorQuaternion& VectorQuaternion::
-conjugate(VectorQuaternion& res) const
-{
+VectorQuaternion &VectorQuaternion::conjugate(VectorQuaternion &res) const {
res.vector(0) = vector(0);
res.vector(1) = -vector(1);
res.vector(2) = -vector(2);
@@ -145,23 +126,22 @@ conjugate(VectorQuaternion& res) const
return res;
}
-VectorQuaternion& VectorQuaternion::
-multiply(const VectorQuaternion& q2, VectorQuaternion& res) const
-{
- double & a1 = vector(0);
- double & b1 = vector(1);
- double & c1 = vector(2);
- double & d1 = vector(3);
-
- double & a2 = q2.vector(0);
- double & b2 = q2.vector(1);
- double & c2 = q2.vector(2);
- double & d2 = q2.vector(3);
-
- res.vector(0) = a1*a2 - b1*b2 - c1*c2 - d1*d2;
- res.vector(1) = a1*b2 + b1*a2 + c1*d2 - d1*c2;
- res.vector(2) = a1*c2 + c1*a2 + d1*b2 - b1*d2;
- res.vector(3) = a1*d2 + d1*a2 + b1*c2 - c1*b2;
+VectorQuaternion &VectorQuaternion::multiply(const VectorQuaternion &q2,
+ VectorQuaternion &res) const {
+ double &a1 = vector(0);
+ double &b1 = vector(1);
+ double &c1 = vector(2);
+ double &d1 = vector(3);
+
+ double &a2 = q2.vector(0);
+ double &b2 = q2.vector(1);
+ double &c2 = q2.vector(2);
+ double &d2 = q2.vector(3);
+
+ res.vector(0) = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2;
+ res.vector(1) = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2;
+ res.vector(2) = a1 * c2 + c1 * a2 + d1 * b2 - b1 * d2;
+ res.vector(3) = a1 * d2 + d1 * a2 + b1 * c2 - c1 * b2;
return res;
}
diff --git a/src/matrix/derivator.cpp b/src/matrix/derivator.cpp
index 0188d092..3032b719 100644
--- a/src/matrix/derivator.cpp
+++ b/src/matrix/derivator.cpp
@@ -22,8 +22,8 @@ using namespace dynamicgraph;
// getTypeName( void ) { return #double; }
// template<>
-// const std::string Derivator<double>::CLASS_NAME = std::string("Derivator_of_double");
-// extern "C" {
+// const std::string Derivator<double>::CLASS_NAME =
+// std::string("Derivator_of_double"); extern "C" {
// Entity *regFunctiondoubleDerivator( const std::string& objname )
// {
// return new Derivator<double>( objname );
@@ -33,45 +33,45 @@ 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 {
- namespace sot {
- 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 sot {
+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
#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): \
- sotClassType<sotType> (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;
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator,Double,"Derivator")
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator,Vector,"Derivator")
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator,Matrix,"Derivator")
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator,VectorQuaternion,"Derivator")
+typedef double Double;
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator, Double, "Derivator")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator, Vector, "Derivator")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator, Matrix, "Derivator")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_WIN32(Derivator, VectorQuaternion,
+ "Derivator")
#endif
diff --git a/src/matrix/fir-filter.cpp b/src/matrix/fir-filter.cpp
index f93ac2f9..b7fc03c1 100644
--- a/src/matrix/fir-filter.cpp
+++ b/src/matrix/fir-filter.cpp
@@ -7,76 +7,82 @@
*
*/
-#include <sot/core/fir-filter.hh>
#include <sot/core/factory.hh>
+#include <sot/core/fir-filter.hh>
-using dynamicgraph::Vector;
-using dynamicgraph::EntityRegisterer;
using dynamicgraph::Entity;
+using dynamicgraph::EntityRegisterer;
+using dynamicgraph::Vector;
-#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(sotClassType,sotSigType,sotCoefType,id,className) \
- template<> \
- std::string sotClassType<sotSigType,sotCoefType>:: \
- getTypeName( void ) { return #sotSigType; } \
- \
- template<> \
- const std::string sotClassType<sotSigType,sotCoefType>::CLASS_NAME \
- = std::string(className)+"_"+#sotSigType+","+#sotCoefType+"_"; \
- \
- template<> \
- const std::string& sotClassType<sotSigType,sotCoefType>:: \
- getClassName( void ) const { return CLASS_NAME; } \
-extern "C" { \
- Entity *regFunction##_##id ( const std::string& objname ) \
- { \
- return new sotClassType<sotSigType,sotCoefType>( objname ); \
- } \
- EntityRegisterer reg##_##id \
- ( std::string(className)+"_"+#sotSigType+"_"+#sotCoefType, \
- ®Function##_##id ); \
-}
+#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(sotClassType, sotSigType, \
+ sotCoefType, id, className) \
+ template <> \
+ std::string sotClassType<sotSigType, sotCoefType>::getTypeName(void) { \
+ return #sotSigType; \
+ } \
+ \
+ template <> \
+ const std::string sotClassType<sotSigType, sotCoefType>::CLASS_NAME = \
+ std::string(className) + "_" + #sotSigType + "," + #sotCoefType + "_"; \
+ \
+ template <> \
+ const std::string &sotClassType<sotSigType, sotCoefType>::getClassName(void) \
+ const { \
+ return CLASS_NAME; \
+ } \
+ extern "C" { \
+ Entity *regFunction##_##id(const std::string &objname) { \
+ return new sotClassType<sotSigType, sotCoefType>(objname); \
+ } \
+ EntityRegisterer reg##_##id(std::string(className) + "_" + #sotSigType + \
+ "_" + #sotCoefType, \
+ ®Function##_##id); \
+ }
namespace dynamicgraph {
- namespace sot {
- using dynamicgraph::command::Value;
- using dynamicgraph::command::Command;
+namespace sot {
+using dynamicgraph::command::Command;
+using dynamicgraph::command::Value;
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(FIRFilter,double,double,double_double,"FIRFilter")
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(FIRFilter,Vector,double,vec_double,"FIRFilter")
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(FIRFilter,Vector,Matrix,vec_mat,"FIRFilter")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(FIRFilter, double, double, double_double,
+ "FIRFilter")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(FIRFilter, Vector, double, vec_double,
+ "FIRFilter")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN(FIRFilter, Vector, Matrix, vec_mat,
+ "FIRFilter")
- template<>
- void FIRFilter<Vector, double>::reset_signal( Vector& res, const Vector& sample )
- {
+template <>
+void FIRFilter<Vector, double>::reset_signal(Vector &res,
+ const Vector &sample) {
res.resize(sample.size());
res.fill(0);
}
-template<>
-void FIRFilter<Vector, Matrix>::reset_signal( Vector& res, const Vector& sample )
-{
+template <>
+void FIRFilter<Vector, Matrix>::reset_signal(Vector &res,
+ const Vector &sample) {
res.resize(sample.size());
res.fill(0);
}
- } // namespace sot
+} // 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): \
- sotClassType<sotSigType,sotCoefType> (name) {}; \
+#define DEFINE_SPECIFICATION(sotClassType, sotSigType, sotCoefType) \
+ sotClassType##sotSigType##sotCoefType:: \
+ sotClassType##sotSigType##sotCoefType(const std::string &name) \
+ : sotClassType<sotSigType, sotCoefType>(name){};
namespace dynamicgraph {
- namespace sot {
+namespace sot {
typedef double Double;
- typedef Value dynamicgraph::command::Value;
- DEFINE_SPECIFICATION(FIRFilter,Double,Double)
- DEFINE_SPECIFICATION(FIRFilter,Vector,Double)
- DEFINE_SPECIFICATION(FIRFilter,Vector,Matrix)
- } // namespace sot
+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
+#endif // WIN32
diff --git a/src/matrix/integrator-abstract.cpp b/src/matrix/integrator-abstract.cpp
index 216bf4a2..eb17fef1 100644
--- a/src/matrix/integrator-abstract.cpp
+++ b/src/matrix/integrator-abstract.cpp
@@ -15,12 +15,13 @@
#include <sot/core/integrator-abstract-impl.hh>
#ifdef WIN32
- IntegratorAbstractDouble::IntegratorAbstractDouble( const std::string& name ) :
- IntegratorAbstract<double,double> (name) {}
+IntegratorAbstractDouble::IntegratorAbstractDouble(const std::string &name)
+ : IntegratorAbstract<double, double>(name) {}
- IntegratorAbstractVector::IntegratorAbstractVector( const std::string& name ) :
- IntegratorAbstract<dynamicgraph::Vector,dynamicgraph::Matrix> (name) {}
+IntegratorAbstractVector::IntegratorAbstractVector(const std::string &name)
+ : IntegratorAbstract<dynamicgraph::Vector, dynamicgraph::Matrix>(name) {}
- IntegratorAbstractVector::IntegratorAbstractVectorDouble( const std::string& name ) :
- IntegratorAbstract<dynamicgraph::Vector,double> (name) {}
+IntegratorAbstractVector::IntegratorAbstractVectorDouble(
+ const std::string &name)
+ : IntegratorAbstract<dynamicgraph::Vector, double>(name) {}
#endif
diff --git a/src/matrix/integrator-euler.cpp b/src/matrix/integrator-euler.cpp
index 0da2e6aa..03416ec0 100644
--- a/src/matrix/integrator-euler.cpp
+++ b/src/matrix/integrator-euler.cpp
@@ -14,11 +14,17 @@
#include <sot/core/integrator-euler-impl.hh>
#ifdef WIN32
- IntegratorEulerVectorMatrix::IntegratorEulerVectorMatrix( const std::string& name ) :
- IntegratorEuler<Vector,Matrix>(name) {}
- std::string IntegratorEulerVectorMatrix::getTypeName( void ) { return "IntegratorEulerVectorMatrix"; }
+IntegratorEulerVectorMatrix::IntegratorEulerVectorMatrix(
+ const std::string &name)
+ : IntegratorEuler<Vector, Matrix>(name) {}
+std::string IntegratorEulerVectorMatrix::getTypeName(void) {
+ return "IntegratorEulerVectorMatrix";
+}
- IntegratorEulerVectorDouble::IntegratorEulerVectorDouble( const std::string& name ) :
- IntegratorEuler<Vector,double>(name) {}
- std::string IntegratorEulerVectorDouble::getTypeName( void ) { return "IntegratorEulerVectorDouble"; }
+IntegratorEulerVectorDouble::IntegratorEulerVectorDouble(
+ const std::string &name)
+ : IntegratorEuler<Vector, double>(name) {}
+std::string IntegratorEulerVectorDouble::getTypeName(void) {
+ return "IntegratorEulerVectorDouble";
+}
#endif // WIN32
diff --git a/src/matrix/integrator-euler.t.cpp b/src/matrix/integrator-euler.t.cpp
index 7bd8b8c4..b8307fb2 100644
--- a/src/matrix/integrator-euler.t.cpp
+++ b/src/matrix/integrator-euler.t.cpp
@@ -7,43 +7,48 @@
*
*/
-#include <sot/core/integrator-euler.hh>
#include <sot/core/factory.hh>
+#include <sot/core/integrator-euler.hh>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(sotClassType,sotSigType,sotCoefType,className) \
- template<> \
- std::string sotClassType<sotSigType,sotCoefType>:: \
- getTypeName( void ) { return #sotSigType; } \
- template<> \
- const std::string sotClassType<sotSigType,sotCoefType>::CLASS_NAME = className; \
- template<> \
- const std::string& sotClassType<sotSigType,sotCoefType>:: \
- getClassName( void ) const { return CLASS_NAME; } \
- extern "C" { \
- Entity *regFunction##_##sotSigType##_##sotCoefType( const std::string& objname ) \
- { \
- return new sotClassType<sotSigType,sotCoefType>( objname ); \
- } \
- EntityRegisterer \
- regObj##_##sotSigType##_##sotCoefType(sotClassType<sotSigType,sotCoefType>::CLASS_NAME, \
- ®Function##_##sotSigType##_##sotCoefType ); \
+#define SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(sotClassType, sotSigType, \
+ sotCoefType, className) \
+ template <> \
+ std::string sotClassType<sotSigType, sotCoefType>::getTypeName(void) { \
+ return #sotSigType; \
+ } \
+ template <> \
+ const std::string sotClassType<sotSigType, sotCoefType>::CLASS_NAME = \
+ className; \
+ template <> \
+ const std::string &sotClassType<sotSigType, sotCoefType>::getClassName(void) \
+ const { \
+ return CLASS_NAME; \
+ } \
+ extern "C" { \
+ Entity * \
+ regFunction##_##sotSigType##_##sotCoefType(const std::string &objname) { \
+ return new sotClassType<sotSigType, sotCoefType>(objname); \
+ } \
+ EntityRegisterer regObj##_##sotSigType##_##sotCoefType( \
+ sotClassType<sotSigType, sotCoefType>::CLASS_NAME, \
+ ®Function##_##sotSigType##_##sotCoefType); \
}
using namespace std;
namespace dynamicgraph {
- namespace sot {
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,double,double,
- "IntegratorEulerDoubleDouble")
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,Vector,Matrix,
- "IntegratorEulerVectorMatrix")
- SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler,Vector,double,
- "IntegratorEulerVectorDouble")
+namespace sot {
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler, double, double,
+ "IntegratorEulerDoubleDouble")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler, Vector, Matrix,
+ "IntegratorEulerVectorMatrix")
+SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_EULER(IntegratorEuler, Vector, double,
+ "IntegratorEulerVectorDouble")
- template class IntegratorEuler<double,double>;
- template class IntegratorEuler<Vector,double>;
- template class IntegratorEuler<Vector,Matrix>;
- } // namespace sot
+template class IntegratorEuler<double, double>;
+template class IntegratorEuler<Vector, double>;
+template class IntegratorEuler<Vector, Matrix>;
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/matrix/matrix-constant-command.h b/src/matrix/matrix-constant-command.h
index 77c9cfbd..9b0f9136 100644
--- a/src/matrix/matrix-constant-command.h
+++ b/src/matrix/matrix-constant-command.h
@@ -7,51 +7,48 @@
*/
#ifndef MATRIX_CONSTANT_COMMAND_H
- #define MATRIX_CONSTANT_COMMAND_H
+#define MATRIX_CONSTANT_COMMAND_H
- #include <boost/assign/list_of.hpp>
+#include <boost/assign/list_of.hpp>
- #include <dynamic-graph/command.h>
- #include <dynamic-graph/command-setter.h>
- #include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command.h>
namespace dynamicgraph {
- namespace sot {
- namespace command {
- namespace matrixConstant {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
+namespace sot {
+namespace command {
+namespace matrixConstant {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
- // Command Resize
- class Resize : public Command
- {
- public:
- virtual ~Resize() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Resize(MatrixConstant& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::UNSIGNED)
- (Value::UNSIGNED), docstring)
- {
- }
- virtual Value doExecute()
- {
- MatrixConstant& mc = static_cast<MatrixConstant&>(owner());
- std::vector<Value> values = getParameterValues();
- unsigned rows = values[0].value();
- unsigned cols = values[1].value();
- dynamicgraph::Matrix m(rows, cols);
- m.fill(mc.color);
- mc.SOUT.setConstant(m);
+// Command Resize
+class Resize : public Command {
+public:
+ virtual ~Resize() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Resize(MatrixConstant &entity, const std::string &docstring)
+ : Command(entity,
+ boost::assign::list_of(Value::UNSIGNED)(Value::UNSIGNED),
+ docstring) {}
+ virtual Value doExecute() {
+ MatrixConstant &mc = static_cast<MatrixConstant &>(owner());
+ std::vector<Value> values = getParameterValues();
+ unsigned rows = values[0].value();
+ unsigned cols = values[1].value();
+ dynamicgraph::Matrix m(rows, cols);
+ m.fill(mc.color);
+ mc.SOUT.setConstant(m);
- // return void
- return Value();
- }
- }; // class Resize
- } // namespace matrixConstant
- } // namespace command
- } // namespace sot
+ // return void
+ return Value();
+ }
+}; // 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 42953fb0..9586dc70 100644
--- a/src/matrix/matrix-constant.cpp
+++ b/src/matrix/matrix-constant.cpp
@@ -7,8 +7,8 @@
*
*/
-#include <sot/core/matrix-constant.hh>
#include <sot/core/factory.hh>
+#include <sot/core/matrix-constant.hh>
#include "../src/matrix/matrix-constant-command.h"
@@ -16,47 +16,43 @@ using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MatrixConstant,"MatrixConstant");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MatrixConstant, "MatrixConstant");
/* --------------------------------------------------------------------- */
/* --- MATRIX ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-MatrixConstant::
-MatrixConstant( const std::string& name )
- :Entity( name )
- ,rows(0),cols(0),color(0.)
- ,SOUT( "sotMatrixConstant("+name+")::output(matrix)::sout" )
-{
- SOUT.setDependencyType( TimeDependency<int>::BOOL_DEPENDENT );
- signalRegistration( SOUT );
+MatrixConstant::MatrixConstant(const std::string &name)
+ : Entity(name), rows(0), cols(0), color(0.),
+ SOUT("sotMatrixConstant(" + name + ")::output(matrix)::sout") {
+ SOUT.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
+ signalRegistration(SOUT);
//
// Commands
// Resize
std::string docstring;
- docstring = " \n"
- " Resize the matrix and fill with value stored in color field.\n"
- " Input\n"
- " - unsigned int: number of lines.\n"
- " - unsigned int: number of columns.\n"
- "\n";
- addCommand("resize",
- new command::matrixConstant::Resize(*this, docstring));
+ docstring =
+ " \n"
+ " Resize the matrix and fill with value stored in color field.\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";
- addCommand("set",
- new ::dynamicgraph::command::Setter<MatrixConstant, dynamicgraph::Matrix>
- (*this, &MatrixConstant::setValue, docstring));
+ " Set value of output signal\n"
+ " \n"
+ " input:\n"
+ " - a matrix\n"
+ " \n";
+ addCommand(
+ "set",
+ new ::dynamicgraph::command::Setter<MatrixConstant, dynamicgraph::Matrix>(
+ *this, &MatrixConstant::setValue, docstring));
}
-void MatrixConstant::
-setValue(const dynamicgraph::Matrix& inValue)
-{
+void MatrixConstant::setValue(const dynamicgraph::Matrix &inValue) {
SOUT.setConstant(inValue);
}
diff --git a/src/matrix/matrix-svd.cpp b/src/matrix/matrix-svd.cpp
index 0cb69275..1f8fdb24 100644
--- a/src/matrix/matrix-svd.cpp
+++ b/src/matrix/matrix-svd.cpp
@@ -1,51 +1,48 @@
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
-
#include <sot/core/debug.hh>
#include <sot/core/matrix-svd.hh>
namespace Eigen {
-void pseudoInverse( dg::Matrix& _inputMatrix,
- dg::Matrix& _inverseMatrix,
- const double threshold) {
+void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
+ const double threshold) {
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
- JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues=svd.singularValues();
+ JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues =
+ svd.singularValues();
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
singularValues_inv.resizeLike(m_singularValues);
- for ( long i=0; i<m_singularValues.size(); ++i) {
- if ( m_singularValues(i) > threshold )
- singularValues_inv(i)=1.0/m_singularValues(i);
- else singularValues_inv(i)=0;
+ for (long i = 0; i < m_singularValues.size(); ++i) {
+ if (m_singularValues(i) > threshold)
+ singularValues_inv(i) = 1.0 / m_singularValues(i);
+ else
+ singularValues_inv(i) = 0;
}
- _inverseMatrix = (svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose());
+ _inverseMatrix = (svd.matrixV() * singularValues_inv.asDiagonal() *
+ svd.matrixU().transpose());
}
-void dampedInverse( const JacobiSVD <dg::Matrix>& svd,
- dg::Matrix& _inverseMatrix,
- const double threshold) {
+void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
+ const double threshold) {
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
- ArrayWrapper<const SV_t> sigmas (svd.singularValues());
+ ArrayWrapper<const SV_t> sigmas(svd.singularValues());
- SV_t sv_inv (sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
+ SV_t sv_inv(sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
const dg::Matrix::Index m = sv_inv.size();
- _inverseMatrix.noalias() =
- ( svd.matrixV().leftCols(m) * sv_inv.asDiagonal() * svd.matrixU().leftCols(m).transpose());
+ _inverseMatrix.noalias() = (svd.matrixV().leftCols(m) * sv_inv.asDiagonal() *
+ svd.matrixU().leftCols(m).transpose());
}
-void dampedInverse( const dg::Matrix& _inputMatrix,
- dg::Matrix& _inverseMatrix,
- dg::Matrix& Uref,
- dg::Vector& Sref,
- dg::Matrix& Vref,
- const double threshold) {
+void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
+ dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
+ const double threshold) {
sotDEBUGIN(15);
- sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
+ sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
- dampedInverse (svd, _inverseMatrix, threshold);
+ dampedInverse(svd, _inverseMatrix, threshold);
Uref = svd.matrixU();
Vref = svd.matrixV();
@@ -54,16 +51,15 @@ void dampedInverse( const dg::Matrix& _inputMatrix,
sotDEBUGOUT(15);
}
-void dampedInverse( const dg::Matrix& _inputMatrix,
- dg::Matrix& _inverseMatrix,
- const double threshold) {
+void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
+ const double threshold) {
sotDEBUGIN(15);
- sotDEBUG(5) << "Input Matrix: "<<_inputMatrix<<std::endl;
+ sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeFullV);
- dampedInverse (svd, _inverseMatrix, threshold);
+ dampedInverse(svd, _inverseMatrix, threshold);
sotDEBUGOUT(15);
}
-}
+} // namespace Eigen
diff --git a/src/matrix/operator.cpp b/src/matrix/operator.cpp
index 6cf6b81a..2f9b7681 100644
--- a/src/matrix/operator.cpp
+++ b/src/matrix/operator.cpp
@@ -11,8 +11,8 @@
#include <boost/function.hpp>
-#include <sot/core/unary-op.hh>
#include <sot/core/binary-op.hh>
+#include <sot/core/unary-op.hh>
#include <sot/core/variadic-op.hh>
#include <sot/core/matrix-geometry.hh>
@@ -20,11 +20,11 @@
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
-#include <dynamic-graph/linear-algebra.h>
-#include <sot/core/factory.hh>
-#include <sot/core/debug.hh>
#include <boost/numeric/conversion/cast.hpp>
#include <deque>
+#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/debug.hh>
+#include <sot/core/factory.hh>
#include "../tools/type-name-helper.hh"
@@ -35,1130 +35,1107 @@ namespace dg = ::dynamicgraph;
/* ---------------------------------------------------------------------------*/
namespace dynamicgraph {
- namespace sot {
- template< typename TypeIn, typename TypeOut >
- struct UnaryOpHeader
- {
- typedef TypeIn Tin;
- typedef TypeOut Tout;
- static const std::string & nameTypeIn(void)
- { return TypeNameHelper<Tin>::typeName; }
- static const std::string & nameTypeOut(void)
- { return TypeNameHelper<Tout>::typeName; }
- void addSpecificCommands(Entity&, Entity::CommandMap_t& ) {}
- virtual std::string getDocString () const {
- return std::string
- ("Undocumented unary operator\n"
- " - input ") + nameTypeIn () +
- std::string ("\n"
- " - output ") + nameTypeOut () +
- std::string ("\n");
- }
- };
- } /* namespace sot */
+namespace sot {
+template <typename TypeIn, typename TypeOut> struct UnaryOpHeader {
+ typedef TypeIn Tin;
+ typedef TypeOut Tout;
+ static const std::string &nameTypeIn(void) {
+ return TypeNameHelper<Tin>::typeName;
+ }
+ static const std::string &nameTypeOut(void) {
+ return TypeNameHelper<Tout>::typeName;
+ }
+ void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
+ virtual std::string getDocString() const {
+ return std::string("Undocumented unary operator\n"
+ " - input ") +
+ nameTypeIn() +
+ std::string("\n"
+ " - output ") +
+ nameTypeOut() + std::string("\n");
+ }
+};
+} /* namespace sot */
} /* namespace dynamicgraph */
+#define ADD_COMMAND(name, def) commandMap.insert(std::make_pair(name, def))
-#define ADD_COMMAND( name,def ) \
- commandMap.insert( std::make_pair( name,def ) )
-
-#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)
+#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)
/* ---------------------------------------------------------------------------*/
/* ---------------------------------------------------------------------------*/
/* ---------------------------------------------------------------------------*/
namespace dynamicgraph {
- namespace sot {
-
- /* ---------------------------------------------------------------------- */
- /* --- ALGEBRA SELECTORS ------------------------------------------------ */
- /* ---------------------------------------------------------------------- */
- struct VectorSelecter
- : public UnaryOpHeader<dg::Vector, dg::Vector>
- {
- void operator()( const Tin& m,Vector& res ) const
- {
- res.resize(size);
- Vector::Index r=0;
- for (std::size_t i = 0; i < idxs.size(); ++i) {
- const Vector::Index& R = idxs[i].first;
- const Vector::Index& nr = idxs[i].second;
- assert( (nr>=0) && (R+nr <= m.size()) );
- res.segment(r,nr) = m.segment(R,nr);
- r += nr;
- }
- assert (r == size);
- }
+namespace sot {
+
+/* ---------------------------------------------------------------------- */
+/* --- ALGEBRA SELECTORS ------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+struct VectorSelecter : public UnaryOpHeader<dg::Vector, dg::Vector> {
+ void operator()(const Tin &m, Vector &res) const {
+ res.resize(size);
+ Vector::Index r = 0;
+ for (std::size_t i = 0; i < idxs.size(); ++i) {
+ const Vector::Index &R = idxs[i].first;
+ const Vector::Index &nr = idxs[i].second;
+ assert((nr >= 0) && (R + nr <= m.size()));
+ res.segment(r, nr) = m.segment(R, nr);
+ r += nr;
+ }
+ assert(r == size);
+ }
- typedef std::pair <Vector::Index,Vector::Index> segment_t;
- typedef std::vector <segment_t> segments_t;
- segments_t idxs;
- Vector::Index size;
-
- void setBounds( const int & m,const int & M )
- { idxs = segments_t (1, segment_t(m, M-m)); size = M-m; }
- void addBounds( const int & m,const int & M )
- { idxs .push_back( segment_t(m, M-m)); size += M-m; }
-
- void addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
- std::string doc;
-
- boost::function< void( const int&, const int& ) > setBound
- = boost::bind( &VectorSelecter::setBounds,this,_1,_2 );
- doc = docCommandVoid2("Set the bound of the selection [m,M[.",
- "int (min)","int (max)");
- ADD_COMMAND( "selec", makeCommandVoid2(ent,setBound,doc) );
- boost::function< void( const int&, const int& ) > addBound
- = boost::bind( &VectorSelecter::addBounds,this,_1,_2 );
- doc = docCommandVoid2("Add a segment to be selected [m,M[.",
- "int (min)","int (max)");
- ADD_COMMAND( "addSelec", makeCommandVoid2(ent,addBound,doc) );
- }
- VectorSelecter () : size (0) {}
- };
- REGISTER_UNARY_OP( VectorSelecter,Selec_of_vector );
-
- /* ---------------------------------------------------------------------- */
- /* --- ALGEBRA SELECTORS ------------------------------------------------ */
- /* ---------------------------------------------------------------------- */
- struct VectorComponent
- : public UnaryOpHeader<dg::Vector, double>
- {
- void operator() (const Tin& m, double& res) const
- {
- assert (index < m.size());
- res = m(index);
- }
+ typedef std::pair<Vector::Index, Vector::Index> segment_t;
+ typedef std::vector<segment_t> segments_t;
+ segments_t idxs;
+ Vector::Index size;
- int index;
- void setIndex (const int & m) { index = m; }
-
- void addSpecificCommands
- (Entity& ent,
- Entity::CommandMap_t& commandMap )
- {
- std::string doc;
-
- boost::function< void( const int& ) > callback
- = boost::bind( &VectorComponent::setIndex,this,_1 );
- doc = command::docCommandVoid1("Set the index of the component.",
- "int (index)");
- ADD_COMMAND( "setIndex",
- command::makeCommandVoid1 (ent, callback, doc));
- }
- virtual std::string getDocString () const
- {
- std::string docString
- ("Select a component of a vector\n"
- " - input vector\n"" - output double");
- return docString;
- }
+ void setBounds(const int &m, const int &M) {
+ idxs = segments_t(1, segment_t(m, M - m));
+ size = M - m;
+ }
+ void addBounds(const int &m, const int &M) {
+ idxs.push_back(segment_t(m, M - m));
+ size += M - m;
+ }
- };
- REGISTER_UNARY_OP (VectorComponent, Component_of_vector);
-
- /* ---------------------------------------------------------------------- */
- struct MatrixSelector
- : public UnaryOpHeader<dg::Matrix, dg::Matrix>
- {
- void operator()( const Matrix& m,Matrix& res ) const
- {
- assert ((imin<=imax)&&(imax<=m.rows()));
- 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);
- }
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ std::string doc;
+
+ boost::function<void(const int &, const int &)> setBound =
+ boost::bind(&VectorSelecter::setBounds, this, _1, _2);
+ doc = docCommandVoid2("Set the bound of the selection [m,M[.", "int (min)",
+ "int (max)");
+ ADD_COMMAND("selec", makeCommandVoid2(ent, setBound, doc));
+ boost::function<void(const int &, const int &)> addBound =
+ boost::bind(&VectorSelecter::addBounds, this, _1, _2);
+ doc = docCommandVoid2("Add a segment to be selected [m,M[.", "int (min)",
+ "int (max)");
+ ADD_COMMAND("addSelec", makeCommandVoid2(ent, addBound, doc));
+ }
+ VectorSelecter() : size(0) {}
+};
+REGISTER_UNARY_OP(VectorSelecter, Selec_of_vector);
+
+/* ---------------------------------------------------------------------- */
+/* --- ALGEBRA SELECTORS ------------------------------------------------ */
+/* ---------------------------------------------------------------------- */
+struct VectorComponent : public UnaryOpHeader<dg::Vector, double> {
+ void operator()(const Tin &m, double &res) const {
+ assert(index < m.size());
+ res = m(index);
+ }
- public:
- int imin,imax;
- int jmin,jmax;
+ int index;
+ void setIndex(const int &m) { index = m; }
- void setBoundsRow( const int & m,const int & M ) { imin = m; imax = M; }
- void setBoundsCol( const int & m,const int & M ) { jmin = m; jmax = M; }
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ std::string doc;
- void addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
- std::string doc;
+ boost::function<void(const int &)> callback =
+ boost::bind(&VectorComponent::setIndex, this, _1);
+ doc = command::docCommandVoid1("Set the index of the component.",
+ "int (index)");
+ ADD_COMMAND("setIndex", command::makeCommandVoid1(ent, callback, doc));
+ }
+ virtual std::string getDocString() const {
+ std::string docString("Select a component of a vector\n"
+ " - input vector\n"
+ " - output double");
+ return docString;
+ }
+};
+REGISTER_UNARY_OP(VectorComponent, Component_of_vector);
+
+/* ---------------------------------------------------------------------- */
+struct MatrixSelector : public UnaryOpHeader<dg::Matrix, dg::Matrix> {
+ void operator()(const Matrix &m, Matrix &res) const {
+ assert((imin <= imax) && (imax <= m.rows()));
+ 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);
+ }
- boost::function< void( const int&, const int& ) > setBoundsRow
- = boost::bind( &MatrixSelector::setBoundsRow,this,_1,_2 );
- boost::function< void( const int&, const int& ) > setBoundsCol
- = boost::bind( &MatrixSelector::setBoundsCol,this,_1,_2 );
+public:
+ int imin, imax;
+ int jmin, jmax;
- doc = docCommandVoid2("Set the bound on rows.","int (min)","int (max)");
- ADD_COMMAND( "selecRows", makeCommandVoid2(ent,setBoundsRow,doc) );
+ void setBoundsRow(const int &m, const int &M) {
+ imin = m;
+ imax = M;
+ }
+ void setBoundsCol(const int &m, const int &M) {
+ jmin = m;
+ jmax = M;
+ }
- doc = docCommandVoid2("Set the bound on cols [m,M[.","int (min)","int (max)");
- ADD_COMMAND( "selecCols", makeCommandVoid2(ent,setBoundsCol,doc) );
- }
- };
- REGISTER_UNARY_OP( MatrixSelector,Selec_of_matrix );
-
- /* ---------------------------------------------------------------------- */
- struct MatrixColumnSelector
- : public UnaryOpHeader<dg::Matrix, dg::Vector>
- {
- public:
- 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);
- }
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ std::string doc;
- int imin,imax;
- int jcol;
- void selectCol( const int & m ) { jcol=m; }
- void setBoundsRow( const int & m,const int & M ) { imin = m; imax = M; }
+ boost::function<void(const int &, const int &)> setBoundsRow =
+ boost::bind(&MatrixSelector::setBoundsRow, this, _1, _2);
+ boost::function<void(const int &, const int &)> setBoundsCol =
+ boost::bind(&MatrixSelector::setBoundsCol, this, _1, _2);
- void addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
- std::string doc;
+ doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)");
+ ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc));
- boost::function< void( const int&, const int& ) > setBoundsRow
- = boost::bind( &MatrixColumnSelector::setBoundsRow,this,_1,_2 );
- boost::function< void( const int& ) > selectCol
- = boost::bind( &MatrixColumnSelector::selectCol,this,_1 );
+ doc = docCommandVoid2("Set the bound on cols [m,M[.", "int (min)",
+ "int (max)");
+ ADD_COMMAND("selecCols", makeCommandVoid2(ent, setBoundsCol, doc));
+ }
+};
+REGISTER_UNARY_OP(MatrixSelector, Selec_of_matrix);
+
+/* ---------------------------------------------------------------------- */
+struct MatrixColumnSelector : public UnaryOpHeader<dg::Matrix, dg::Vector> {
+public:
+ 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);
+ }
- doc = docCommandVoid2("Set the bound on rows.","int (min)","int (max)");
- ADD_COMMAND( "selecRows", makeCommandVoid2(ent,setBoundsRow,doc) );
+ int imin, imax;
+ int jcol;
+ void selectCol(const int &m) { jcol = m; }
+ void setBoundsRow(const int &m, const int &M) {
+ imin = m;
+ imax = M;
+ }
- doc = docCommandVoid1("Select the col to copy.","int (col index)");
- ADD_COMMAND( "selecCols", makeCommandVoid1(ent,selectCol,doc) );
- }
- };
- REGISTER_UNARY_OP( MatrixColumnSelector,Selec_column_of_matrix );
-
-
- /* ---------------------------------------------------------------------- */
- struct MatrixTranspose
- : public UnaryOpHeader<dg::Matrix,dg::Matrix>
- {
- void operator()( const Tin& m,Tout& res )
- const { res = m.transpose(); }
- };
- REGISTER_UNARY_OP( MatrixTranspose, MatrixTranspose);
-
- /* ---------------------------------------------------------------------- */
- struct Diagonalizer
- : public UnaryOpHeader<Vector,Matrix>
- {
- void operator()( const dg::Vector& r,dg::Matrix & res )
- {
- res = r.asDiagonal();
- }
- public:
- Diagonalizer( void ) : nbr(0),nbc(0) {}
- unsigned int nbr, nbc;
- void resize( const int & r, const int & c ) { nbr=r;nbc=c; }
- void addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
- std::string doc;
-
- boost::function< void( const int&, const int& ) > resize
- = boost::bind( &Diagonalizer::resize,this,_1,_2 );
-
- doc = docCommandVoid2("Set output size.","int (row)","int (col)");
- ADD_COMMAND( "resize", makeCommandVoid2(ent,resize,doc) );
- }
- };
- REGISTER_UNARY_OP(Diagonalizer,MatrixDiagonal);
-
- /* ---------------------------------------------------------------------- */
- /* --- INVERSION -------------------------------------------------------- */
- /* ---------------------------------------------------------------------- */
-
- template< typename matrixgen >
- struct Inverser
- : public UnaryOpHeader<matrixgen,matrixgen>
- {
- typedef typename UnaryOpHeader<matrixgen,matrixgen>::Tin Tin;
- typedef typename UnaryOpHeader<matrixgen,matrixgen>::Tout Tout;
- void operator()( const Tin& m,Tout& res ) const
- { res = m.inverse(); }
- };
-
- REGISTER_UNARY_OP( Inverser<dg::Matrix>, Inverse_of_matrix);
- REGISTER_UNARY_OP( Inverser<MatrixHomogeneous>, Inverse_of_matrixHomo);
- REGISTER_UNARY_OP( Inverser<MatrixTwist>, Inverse_of_matrixtwist);
-
-
-
- struct Normalize
- : public UnaryOpHeader<dg::Vector,double>
- {
- void operator()( const dg::Vector& m, double& res ) const
- { res = m.norm(); }
-
- virtual std::string getDocString () const
- {
- std::string docString
- ("Computes the norm of a vector\n"
- " - input vector\n"" - output double");
- return docString;
- }
- };
- REGISTER_UNARY_OP( Normalize, Norm_of_vector);
-
-
- struct InverserRotation
- : public UnaryOpHeader<MatrixRotation,MatrixRotation>
- {
- void operator()( const Tin& m,Tout& res )
- const { res = m.transpose(); }
- };
- REGISTER_UNARY_OP( InverserRotation, Inverse_of_matrixrotation);
-
- struct InverserQuaternion
- : public UnaryOpHeader<VectorQuaternion,VectorQuaternion>
- {
- void operator()( const Tin& m,Tout& res )
- const { res = m.conjugate(); }
- };
- REGISTER_UNARY_OP( InverserQuaternion, Inverse_of_unitquat);
-
- /* ----------------------------------------------------------------------- */
- /* --- SE3/SO3 conversions ----------------------------------------------- */
- /* ----------------------------------------------------------------------- */
-
- struct HomogeneousMatrixToVector
- : public UnaryOpHeader<MatrixHomogeneous,dg::Vector>
- {
- void operator()( const MatrixHomogeneous& M,dg::Vector& res )
- {
- res.resize(6);
- VectorUTheta r(M.linear());
- res.head<3>() = M.translation();
- res.tail<3>() = r.angle()*r.axis();
- }
- };
- REGISTER_UNARY_OP( HomogeneousMatrixToVector,MatrixHomoToPoseUTheta);
-
- struct SkewSymToVector
- : public UnaryOpHeader<Matrix,Vector>
- {
- void operator()( const Matrix& M,Vector& res )
- {
- res.resize(3);
- res(0) = M(7);
- res(1) = M(2);
- res(2) = M(3);
- }
- };
- REGISTER_UNARY_OP( SkewSymToVector,SkewSymToVector );
-
- struct PoseUThetaToMatrixHomo
- : public UnaryOpHeader<Vector,MatrixHomogeneous>
- {
- void operator()( const dg::Vector& v,MatrixHomogeneous& res )
- {
- assert( v.size()>=6 );
- res.translation() = v.head<3>();
- double theta = v.tail<3>().norm();
- if (theta > 0)
- res.linear() = Eigen::AngleAxisd(theta, v.tail<3>() / theta).matrix();
- else
- res.linear().setIdentity();
- }
- };
- REGISTER_UNARY_OP(PoseUThetaToMatrixHomo,PoseUThetaToMatrixHomo);
-
- struct MatrixHomoToPoseQuaternion
- : public UnaryOpHeader<MatrixHomogeneous,Vector>
- {
- void operator()( const MatrixHomogeneous& M,Vector& res )
- {
- res.resize(7);
- res.head<3>() = M.translation();
- Eigen::Map<VectorQuaternion> q (res.tail<4>().data());
- q = M.linear();
- }
- };
- REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion,MatrixHomoToPoseQuaternion);
-
- struct MatrixHomoToPoseRollPitchYaw
- : public UnaryOpHeader<MatrixHomogeneous,Vector>
- {
- void operator()( const MatrixHomogeneous& M,dg::Vector& res )
- {
- VectorRollPitchYaw r = (M.linear().eulerAngles(2,1,0)).reverse();
- 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);
- }
- };
- REGISTER_UNARY_OP(MatrixHomoToPoseRollPitchYaw,MatrixHomoToPoseRollPitchYaw);
-
- struct PoseRollPitchYawToMatrixHomo
- : public UnaryOpHeader<Vector,MatrixHomogeneous>
- {
- void operator()( const dg::Vector& vect, MatrixHomogeneous& Mres )
- {
-
- VectorRollPitchYaw r;
- 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);
-
- //buildFrom(R,t);
- Mres = Eigen::Translation3d(t)*R;
- }
- };
- REGISTER_UNARY_OP(PoseRollPitchYawToMatrixHomo,PoseRollPitchYawToMatrixHomo);
-
- struct PoseRollPitchYawToPoseUTheta
- : public UnaryOpHeader<Vector,Vector>
- {
- void operator()( const dg::Vector& vect,dg::Vector& vectres )
- {
- VectorRollPitchYaw r;
- 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();
-
- VectorUTheta rrot(R);
-
- vectres .resize(6);
- for( unsigned int i=0;i<3;++i )
- {
- vectres(i)=vect(i);
- vectres (i+3) = rrot.angle()*rrot.axis()(i);
- }
- }
- };
- REGISTER_UNARY_OP(PoseRollPitchYawToPoseUTheta,PoseRollPitchYawToPoseUTheta);
-
- struct HomoToMatrix
- : public UnaryOpHeader<MatrixHomogeneous,Matrix>
- {
- void operator()( const MatrixHomogeneous& M,dg::Matrix& res )
- { res=M.matrix(); }
- };
- REGISTER_UNARY_OP(HomoToMatrix,HomoToMatrix);
-
- struct MatrixToHomo
- : public UnaryOpHeader<Matrix,MatrixHomogeneous>
- {
- void operator()( const Eigen::Matrix<double,4,4>& M,MatrixHomogeneous& res )
- { res= M; }
- };
- REGISTER_UNARY_OP(MatrixToHomo,MatrixToHomo);
-
- struct HomoToTwist
- : public UnaryOpHeader<MatrixHomogeneous,MatrixTwist>
- {
- void operator()( const MatrixHomogeneous& M,MatrixTwist& res )
- {
-
- Eigen::Vector3d _t = M.translation();
- MatrixRotation R(M.linear());
- Eigen::Matrix3d Tx;
- Tx << 0, -_t(2), _t(1),
- _t(2), 0, -_t(0),
- -_t(1), _t(0), 0;
-
- Eigen::Matrix3d sk; sk = Tx*R;
- res.block<3,3>(0,0) = R;
- res.block<3,3>(0,3) = sk;
- res.block<3,3>(3,0) = Eigen::Matrix3d::Zero();
- res.block<3,3>(3,3) = R;
- }
- };
- REGISTER_UNARY_OP(HomoToTwist,HomoToTwist);
-
- struct HomoToRotation
- : public UnaryOpHeader<MatrixHomogeneous,MatrixRotation>
- {
- void operator()( const MatrixHomogeneous& M,MatrixRotation& res )
- {
- res = M.linear();
- }
- };
- REGISTER_UNARY_OP(HomoToRotation,HomoToRotation);
-
- struct MatrixHomoToPose
- : public UnaryOpHeader<MatrixHomogeneous,Vector>
- {
- void operator()( const MatrixHomogeneous& M,Vector& res )
- {
- res.resize(3);
- res = M.translation();
- }
- };
- REGISTER_UNARY_OP(MatrixHomoToPose,MatrixHomoToPose);
-
- struct RPYToMatrix
- : public UnaryOpHeader<VectorRollPitchYaw,MatrixRotation>
- {
- void operator()( const VectorRollPitchYaw& r,MatrixRotation& res )
- {
- res = (Eigen::AngleAxisd(r(2),Eigen::Vector3d::UnitZ())*
- Eigen::AngleAxisd(r(1),Eigen::Vector3d::UnitY())*
- Eigen::AngleAxisd(r(0),Eigen::Vector3d::UnitX())).toRotationMatrix();
- }
- };
- REGISTER_UNARY_OP(RPYToMatrix,RPYToMatrix);
-
- struct MatrixToRPY
- : public UnaryOpHeader<MatrixRotation,VectorRollPitchYaw>
- {
- void operator()( const MatrixRotation& r,VectorRollPitchYaw & res )
- {
- res = (r.eulerAngles(2,1,0)).reverse();
- }
- };
- REGISTER_UNARY_OP(MatrixToRPY,MatrixToRPY);
-
- struct QuaternionToMatrix
- : public UnaryOpHeader<VectorQuaternion,MatrixRotation>
- {
- void operator()( const VectorQuaternion& r,MatrixRotation& res )
- {
- res = r.toRotationMatrix();
- }
- };
- REGISTER_UNARY_OP(QuaternionToMatrix,QuaternionToMatrix);
-
- struct MatrixToQuaternion
- : public UnaryOpHeader<MatrixRotation,VectorQuaternion>
- {
- void operator()( const MatrixRotation& r,VectorQuaternion & res )
- {
- res = r;
- }
- };
- REGISTER_UNARY_OP(MatrixToQuaternion,MatrixToQuaternion);
-
- struct MatrixToUTheta
- : public UnaryOpHeader<MatrixRotation,VectorUTheta>
- {
- void operator()( const MatrixRotation& r,VectorUTheta & res )
- {
- res = r;
- }
- };
- REGISTER_UNARY_OP(MatrixToUTheta,MatrixToUTheta);
-
- struct UThetaToQuaternion
- : public UnaryOpHeader<VectorUTheta,VectorQuaternion>
- {
- void operator()( const VectorUTheta& r,VectorQuaternion& res )
- {
- res = r;
- }
- };
- REGISTER_UNARY_OP(UThetaToQuaternion,UThetaToQuaternion);
-
- template< typename TypeIn1,typename TypeIn2, typename TypeOut >
- struct BinaryOpHeader
- {
- typedef TypeIn1 Tin1;
- typedef TypeIn2 Tin2;
- typedef TypeOut Tout;
- static const std::string & nameTypeIn1(void) { return TypeNameHelper<Tin1>::typeName; }
- static const std::string & nameTypeIn2(void) { return TypeNameHelper<Tin2>::typeName; }
- static const std::string & nameTypeOut(void) { return TypeNameHelper<Tout>::typeName; }
- void addSpecificCommands(Entity&, Entity::CommandMap_t& ) {}
- virtual std::string getDocString () const
- {
- return std::string
- ("Undocumented binary operator\n"
- " - input ") + nameTypeIn1 () +
- std::string ("\n"
- " - ") + nameTypeIn2 () +
- std::string ("\n"
- " - output ") + nameTypeOut () +
- std::string ("\n");
- }
- };
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ std::string doc;
+ boost::function<void(const int &, const int &)> setBoundsRow =
+ boost::bind(&MatrixColumnSelector::setBoundsRow, this, _1, _2);
+ boost::function<void(const int &)> selectCol =
+ boost::bind(&MatrixColumnSelector::selectCol, this, _1);
- } /* namespace sot */
-} /* namespace dynamicgraph */
+ doc = docCommandVoid2("Set the bound on rows.", "int (min)", "int (max)");
+ ADD_COMMAND("selecRows", makeCommandVoid2(ent, setBoundsRow, doc));
+ doc = docCommandVoid1("Select the col to copy.", "int (col index)");
+ ADD_COMMAND("selecCols", makeCommandVoid1(ent, selectCol, doc));
+ }
+};
+REGISTER_UNARY_OP(MatrixColumnSelector, Selec_column_of_matrix);
+
+/* ---------------------------------------------------------------------- */
+struct MatrixTranspose : public UnaryOpHeader<dg::Matrix, dg::Matrix> {
+ void operator()(const Tin &m, Tout &res) const { res = m.transpose(); }
+};
+REGISTER_UNARY_OP(MatrixTranspose, MatrixTranspose);
+
+/* ---------------------------------------------------------------------- */
+struct Diagonalizer : public UnaryOpHeader<Vector, Matrix> {
+ void operator()(const dg::Vector &r, dg::Matrix &res) {
+ res = r.asDiagonal();
+ }
-#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)
+public:
+ Diagonalizer(void) : nbr(0), nbc(0) {}
+ unsigned int nbr, nbc;
+ void resize(const int &r, const int &c) {
+ nbr = r;
+ nbc = c;
+ }
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ std::string doc;
-/* ---------------------------------------------------------------------------*/
-/* ---------------------------------------------------------------------------*/
-/* ---------------------------------------------------------------------------*/
+ boost::function<void(const int &, const int &)> resize =
+ boost::bind(&Diagonalizer::resize, this, _1, _2);
+ doc = docCommandVoid2("Set output size.", "int (row)", "int (col)");
+ ADD_COMMAND("resize", makeCommandVoid2(ent, resize, doc));
+ }
+};
+REGISTER_UNARY_OP(Diagonalizer, MatrixDiagonal);
+
+/* ---------------------------------------------------------------------- */
+/* --- INVERSION -------------------------------------------------------- */
+/* ---------------------------------------------------------------------- */
+
+template <typename matrixgen>
+struct Inverser : public UnaryOpHeader<matrixgen, matrixgen> {
+ typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tin Tin;
+ typedef typename UnaryOpHeader<matrixgen, matrixgen>::Tout Tout;
+ void operator()(const Tin &m, Tout &res) const { res = m.inverse(); }
+};
+
+REGISTER_UNARY_OP(Inverser<dg::Matrix>, Inverse_of_matrix);
+REGISTER_UNARY_OP(Inverser<MatrixHomogeneous>, Inverse_of_matrixHomo);
+REGISTER_UNARY_OP(Inverser<MatrixTwist>, Inverse_of_matrixtwist);
+
+struct Normalize : public UnaryOpHeader<dg::Vector, double> {
+ void operator()(const dg::Vector &m, double &res) const { res = m.norm(); }
+
+ virtual std::string getDocString() const {
+ std::string docString("Computes the norm of a vector\n"
+ " - input vector\n"
+ " - output double");
+ return docString;
+ }
+};
+REGISTER_UNARY_OP(Normalize, Norm_of_vector);
+
+struct InverserRotation : public UnaryOpHeader<MatrixRotation, MatrixRotation> {
+ void operator()(const Tin &m, Tout &res) const { res = m.transpose(); }
+};
+REGISTER_UNARY_OP(InverserRotation, Inverse_of_matrixrotation);
+
+struct InverserQuaternion
+ : public UnaryOpHeader<VectorQuaternion, VectorQuaternion> {
+ void operator()(const Tin &m, Tout &res) const { res = m.conjugate(); }
+};
+REGISTER_UNARY_OP(InverserQuaternion, Inverse_of_unitquat);
+
+/* ----------------------------------------------------------------------- */
+/* --- SE3/SO3 conversions ----------------------------------------------- */
+/* ----------------------------------------------------------------------- */
+
+struct HomogeneousMatrixToVector
+ : public UnaryOpHeader<MatrixHomogeneous, dg::Vector> {
+ void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
+ res.resize(6);
+ VectorUTheta r(M.linear());
+ res.head<3>() = M.translation();
+ res.tail<3>() = r.angle() * r.axis();
+ }
+};
+REGISTER_UNARY_OP(HomogeneousMatrixToVector, MatrixHomoToPoseUTheta);
+
+struct SkewSymToVector : public UnaryOpHeader<Matrix, Vector> {
+ void operator()(const Matrix &M, Vector &res) {
+ res.resize(3);
+ res(0) = M(7);
+ res(1) = M(2);
+ res(2) = M(3);
+ }
+};
+REGISTER_UNARY_OP(SkewSymToVector, SkewSymToVector);
+
+struct PoseUThetaToMatrixHomo
+ : public UnaryOpHeader<Vector, MatrixHomogeneous> {
+ void operator()(const dg::Vector &v, MatrixHomogeneous &res) {
+ assert(v.size() >= 6);
+ res.translation() = v.head<3>();
+ double theta = v.tail<3>().norm();
+ if (theta > 0)
+ res.linear() = Eigen::AngleAxisd(theta, v.tail<3>() / theta).matrix();
+ else
+ res.linear().setIdentity();
+ }
+};
+REGISTER_UNARY_OP(PoseUThetaToMatrixHomo, PoseUThetaToMatrixHomo);
+
+struct MatrixHomoToPoseQuaternion
+ : public UnaryOpHeader<MatrixHomogeneous, Vector> {
+ void operator()(const MatrixHomogeneous &M, Vector &res) {
+ res.resize(7);
+ res.head<3>() = M.translation();
+ Eigen::Map<VectorQuaternion> q(res.tail<4>().data());
+ q = M.linear();
+ }
+};
+REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion, MatrixHomoToPoseQuaternion);
+
+struct MatrixHomoToPoseRollPitchYaw
+ : public UnaryOpHeader<MatrixHomogeneous, Vector> {
+ void operator()(const MatrixHomogeneous &M, dg::Vector &res) {
+ VectorRollPitchYaw r = (M.linear().eulerAngles(2, 1, 0)).reverse();
+ 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);
+ }
+};
+REGISTER_UNARY_OP(MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseRollPitchYaw);
+
+struct PoseRollPitchYawToMatrixHomo
+ : public UnaryOpHeader<Vector, MatrixHomogeneous> {
+ void operator()(const dg::Vector &vect, MatrixHomogeneous &Mres) {
+
+ VectorRollPitchYaw r;
+ 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);
+
+ // buildFrom(R,t);
+ Mres = Eigen::Translation3d(t) * R;
+ }
+};
+REGISTER_UNARY_OP(PoseRollPitchYawToMatrixHomo, PoseRollPitchYawToMatrixHomo);
+
+struct PoseRollPitchYawToPoseUTheta : public UnaryOpHeader<Vector, Vector> {
+ void operator()(const dg::Vector &vect, dg::Vector &vectres) {
+ VectorRollPitchYaw r;
+ 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();
+
+ VectorUTheta rrot(R);
+
+ vectres.resize(6);
+ for (unsigned int i = 0; i < 3; ++i) {
+ vectres(i) = vect(i);
+ vectres(i + 3) = rrot.angle() * rrot.axis()(i);
+ }
+ }
+};
+REGISTER_UNARY_OP(PoseRollPitchYawToPoseUTheta, PoseRollPitchYawToPoseUTheta);
-namespace dynamicgraph {
- namespace sot {
-
- /* --- MULTIPLICATION --------------------------------------------------- */
-
- template< typename F,typename E>
- struct Multiplier_FxE__E
- : public BinaryOpHeader<F,E,E>
- {
- void operator()( const F& f,const E& e, E& res ) const { res = f*e; }
- };
-
- template<>
- void Multiplier_FxE__E<dynamicgraph::sot::MatrixHomogeneous,dynamicgraph::Vector>::
- operator()( const dynamicgraph::sot::MatrixHomogeneous& f,
- const dynamicgraph::Vector& e,
- dynamicgraph::Vector& res ) const
- { res=f.matrix()*e; }
-
- template<> void Multiplier_FxE__E<double,dynamicgraph::Vector>::
- operator()( const double& x,const dynamicgraph::Vector& v,dynamicgraph::Vector& res ) const
- { res=v; res*=x; }
-
- typedef Multiplier_FxE__E<double,dynamicgraph::Vector> Multiplier_double_vector;
- typedef Multiplier_FxE__E<dynamicgraph::Matrix,dynamicgraph::Vector> Multiplier_matrix_vector;
- typedef Multiplier_FxE__E<MatrixHomogeneous,dynamicgraph::Vector> Multiplier_matrixHomo_vector;
- REGISTER_BINARY_OP( Multiplier_double_vector,Multiply_double_vector);
- REGISTER_BINARY_OP( Multiplier_matrix_vector,Multiply_matrix_vector);
- REGISTER_BINARY_OP( Multiplier_matrixHomo_vector,Multiply_matrixHomo_vector);
-
- /* --- SUBSTRACTION ----------------------------------------------------- */
- template< typename T>
- struct Substraction
- : public BinaryOpHeader<T,T,T>
- { void operator()( const T& v1,const T& v2,T& r ) const { r=v1; r-=v2; } };
-
- REGISTER_BINARY_OP(Substraction<dynamicgraph::Matrix>,Substract_of_matrix);
- REGISTER_BINARY_OP(Substraction<dynamicgraph::Vector>,Substract_of_vector);
- REGISTER_BINARY_OP(Substraction<double>,Substract_of_double);
-
- /* --- STACK ------------------------------------------------------------ */
- struct VectorStack
- : public BinaryOpHeader<dynamicgraph::Vector,dynamicgraph::Vector,dynamicgraph::Vector>
- {
- public:
- int v1min,v1max;
- int v2min,v2max;
- void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& v2,dynamicgraph::Vector& res ) const
- {
- assert( (v1max>=v1min)&&(v1.size()>=v1max) );
- assert( (v2max>=v2min)&&(v2.size()>=v2max) );
-
- const int v1size = v1max-v1min, v2size = v2max-v2min;
- res.resize( v1size+v2size );
- for( int i=0;i<v1size;++i ) { res(i) = v1(i+v1min); }
- for( int i=0;i<v2size;++i ) { res(v1size+i) = v2(i+v2min); }
- }
+struct HomoToMatrix : public UnaryOpHeader<MatrixHomogeneous, Matrix> {
+ void operator()(const MatrixHomogeneous &M, dg::Matrix &res) {
+ res = M.matrix();
+ }
+};
+REGISTER_UNARY_OP(HomoToMatrix, HomoToMatrix);
- void selec1( const int & m, const int M) { v1min=m; v1max=M; }
- void selec2( const int & m, const int M) { v2min=m; v2max=M; }
-
- void addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
- std::string doc;
-
- boost::function< void( const int&, const int& ) > selec1
- = boost::bind( &VectorStack::selec1,this,_1,_2 );
- boost::function< void( const int&, const int& ) > selec2
- = boost::bind( &VectorStack::selec2,this,_1,_2 );
-
- ADD_COMMAND( "selec1",
- makeCommandVoid2(ent,selec1,docCommandVoid2("set the min and max of selection.",
- "int (imin)","int (imax)")));
- ADD_COMMAND( "selec2",
- makeCommandVoid2(ent,selec2,docCommandVoid2("set the min and max of selection.",
- "int (imin)","int (imax)")));
- }
- };
- REGISTER_BINARY_OP(VectorStack,Stack_of_vector);
-
- /* ---------------------------------------------------------------------- */
-
- struct Composer
- : public BinaryOpHeader<dynamicgraph::Matrix,dynamicgraph::Vector,MatrixHomogeneous>
- {
- void operator() ( const dynamicgraph::Matrix& R,const dynamicgraph::Vector& t, MatrixHomogeneous& H ) const
- {
- H.linear() = R;
- H.translation() = t;
- }
- };
- REGISTER_BINARY_OP(Composer,Compose_R_and_T);
-
- /* --- CONVOLUTION PRODUCT ---------------------------------------------- */
- struct ConvolutionTemporal
- : public BinaryOpHeader<dynamicgraph::Vector,dynamicgraph::Matrix,dynamicgraph::Vector>
- {
- typedef std::deque<dynamicgraph::Vector> MemoryType;
- MemoryType memory;
-
- void convolution( const MemoryType &f1,const dynamicgraph::Matrix & f2,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
-
- res.resize( nsig ); res.fill(0);
- unsigned int j=0;
- for( MemoryType::const_iterator iter=f1.begin();iter!=f1.end();iter++ )
- {
- const dynamicgraph::Vector & s_tau = *iter;
- sotDEBUG(45) << "Sig"<<j<< ": " << s_tau ;
- if( s_tau.size()!=nsig )
- return; // TODO: error throw;
- for( int i=0;i<nsig;++i )
- {
- res(i)+=f2(i,j)*s_tau(i);
- }
- j++;
- }
- }
- void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Matrix& m2,dynamicgraph::Vector& res )
- {
- memory.push_front( v1 );
- while( (Vector::Index)memory.size() > m2.cols() ) memory.pop_back();
- convolution( memory,m2,res );
- }
- };
- REGISTER_BINARY_OP( ConvolutionTemporal,ConvolutionTemporal );
+struct MatrixToHomo : public UnaryOpHeader<Matrix, MatrixHomogeneous> {
+ void operator()(const Eigen::Matrix<double, 4, 4> &M,
+ MatrixHomogeneous &res) {
+ res = M;
+ }
+};
+REGISTER_UNARY_OP(MatrixToHomo, MatrixToHomo);
+
+struct HomoToTwist : public UnaryOpHeader<MatrixHomogeneous, MatrixTwist> {
+ void operator()(const MatrixHomogeneous &M, MatrixTwist &res) {
+
+ Eigen::Vector3d _t = M.translation();
+ MatrixRotation R(M.linear());
+ Eigen::Matrix3d Tx;
+ Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
+
+ Eigen::Matrix3d sk;
+ sk = Tx * R;
+ res.block<3, 3>(0, 0) = R;
+ res.block<3, 3>(0, 3) = sk;
+ res.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero();
+ res.block<3, 3>(3, 3) = R;
+ }
+};
+REGISTER_UNARY_OP(HomoToTwist, HomoToTwist);
- /* --- BOOLEAN REDUCTION ------------------------------------------------ */
+struct HomoToRotation
+ : public UnaryOpHeader<MatrixHomogeneous, MatrixRotation> {
+ void operator()(const MatrixHomogeneous &M, MatrixRotation &res) {
+ res = M.linear();
+ }
+};
+REGISTER_UNARY_OP(HomoToRotation, HomoToRotation);
- template < typename T > struct Comparison
- : public BinaryOpHeader <T, T, bool>
- {
- void operator()( const T& a,const T& b, bool& res ) const
- {
- res = ( a < b);
- }
- virtual std::string getDocString () const
- {
- typedef BinaryOpHeader<T,T,bool> Base;
- return std::string
- ("Comparison of inputs:\n"
- " - input ") + Base::nameTypeIn1 () +
- std::string ("\n"
- " - ") + Base::nameTypeIn2 () +
- std::string ("\n"
- " - output ") + Base::nameTypeOut () +
- std::string ("\n"" sout = ( sin1 < sin2 )\n");
- }
- };
-
- template < typename T1, typename T2 = T1 > struct MatrixComparison
- : public BinaryOpHeader <T1, T2, bool>
- {
- // TODO T1 or T2 could be a scalar type.
- void operator()( const T1& a,const T2& b, bool& res ) const
- {
- if ( equal && any) res = (a.array() <= b.array()).any();
- else if ( equal && !any) res = (a.array() <= b.array()).all();
- else if (!equal && any) res = (a.array() < b.array()).any();
- else if (!equal && !any) res = (a.array() < b.array()).all();
- }
- virtual std::string getDocString () const
- {
- typedef BinaryOpHeader<T1,T2,bool> Base;
- return std::string
- ("Comparison of inputs:\n"
- " - input ") + Base::nameTypeIn1 () +
- std::string ("\n"
- " - ") + Base::nameTypeIn2 () +
- 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");
- }
- MatrixComparison () : any (true), equal (false) {}
- void addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap)
- {
- using namespace dynamicgraph::command;
- ADD_COMMAND( "setTrueIfAny",
- makeDirectSetter(ent,&any,docDirectSetter("trueIfAny","bool")));
- ADD_COMMAND( "getTrueIfAny",
- makeDirectGetter(ent,&any,docDirectGetter("trueIfAny","bool")));
- ADD_COMMAND( "setEqual",
- makeDirectSetter(ent,&equal,docDirectSetter("equal","bool")));
- ADD_COMMAND( "getEqual",
- makeDirectGetter(ent,&equal,docDirectGetter("equal","bool")));
- }
- bool any, equal;
- };
+struct MatrixHomoToPose : public UnaryOpHeader<MatrixHomogeneous, Vector> {
+ void operator()(const MatrixHomogeneous &M, Vector &res) {
+ res.resize(3);
+ res = M.translation();
+ }
+};
+REGISTER_UNARY_OP(MatrixHomoToPose, MatrixHomoToPose);
+
+struct RPYToMatrix : public UnaryOpHeader<VectorRollPitchYaw, MatrixRotation> {
+ void operator()(const VectorRollPitchYaw &r, MatrixRotation &res) {
+ res = (Eigen::AngleAxisd(r(2), Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd(r(1), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(r(0), Eigen::Vector3d::UnitX()))
+ .toRotationMatrix();
+ }
+};
+REGISTER_UNARY_OP(RPYToMatrix, RPYToMatrix);
+
+struct MatrixToRPY : public UnaryOpHeader<MatrixRotation, VectorRollPitchYaw> {
+ void operator()(const MatrixRotation &r, VectorRollPitchYaw &res) {
+ res = (r.eulerAngles(2, 1, 0)).reverse();
+ }
+};
+REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY);
+
+struct QuaternionToMatrix
+ : public UnaryOpHeader<VectorQuaternion, MatrixRotation> {
+ void operator()(const VectorQuaternion &r, MatrixRotation &res) {
+ res = r.toRotationMatrix();
+ }
+};
+REGISTER_UNARY_OP(QuaternionToMatrix, QuaternionToMatrix);
+
+struct MatrixToQuaternion
+ : public UnaryOpHeader<MatrixRotation, VectorQuaternion> {
+ void operator()(const MatrixRotation &r, VectorQuaternion &res) { res = r; }
+};
+REGISTER_UNARY_OP(MatrixToQuaternion, MatrixToQuaternion);
+
+struct MatrixToUTheta : public UnaryOpHeader<MatrixRotation, VectorUTheta> {
+ void operator()(const MatrixRotation &r, VectorUTheta &res) { res = r; }
+};
+REGISTER_UNARY_OP(MatrixToUTheta, MatrixToUTheta);
+
+struct UThetaToQuaternion
+ : public UnaryOpHeader<VectorUTheta, VectorQuaternion> {
+ void operator()(const VectorUTheta &r, VectorQuaternion &res) { res = r; }
+};
+REGISTER_UNARY_OP(UThetaToQuaternion, UThetaToQuaternion);
+
+template <typename TypeIn1, typename TypeIn2, typename TypeOut>
+struct BinaryOpHeader {
+ typedef TypeIn1 Tin1;
+ typedef TypeIn2 Tin2;
+ typedef TypeOut Tout;
+ static const std::string &nameTypeIn1(void) {
+ return TypeNameHelper<Tin1>::typeName;
+ }
+ static const std::string &nameTypeIn2(void) {
+ return TypeNameHelper<Tin2>::typeName;
+ }
+ static const std::string &nameTypeOut(void) {
+ return TypeNameHelper<Tout>::typeName;
+ }
+ void addSpecificCommands(Entity &, Entity::CommandMap_t &) {}
+ virtual std::string getDocString() const {
+ return std::string("Undocumented binary operator\n"
+ " - input ") +
+ nameTypeIn1() +
+ std::string("\n"
+ " - ") +
+ nameTypeIn2() +
+ std::string("\n"
+ " - output ") +
+ nameTypeOut() + std::string("\n");
+ }
+};
- REGISTER_BINARY_OP (Comparison<double>, CompareDouble);
- REGISTER_BINARY_OP (MatrixComparison<Vector>, CompareVector);
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
+#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)
+/* ---------------------------------------------------------------------------*/
+/* ---------------------------------------------------------------------------*/
+/* ---------------------------------------------------------------------------*/
namespace dynamicgraph {
- namespace sot {
-
- template< typename T>
- struct WeightedAdder
- : public BinaryOpHeader<T,T,T>
- {
- public:
- double gain1,gain2;
- void operator()( const T& v1,const T& v2, T& res ) const
- {
- res=v1; res*=gain1;
- res += gain2*v2;
- }
+namespace sot {
- void addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap)
- {
- using namespace dynamicgraph::command;
- std::string doc;
-
- ADD_COMMAND( "setGain1",
- makeDirectSetter(ent,&gain1,docDirectSetter("gain1","double")));
- ADD_COMMAND( "setGain2",
- makeDirectSetter(ent,&gain2,docDirectSetter("gain2","double")));
- ADD_COMMAND( "getGain1",
- makeDirectGetter(ent,&gain1,docDirectGetter("gain1","double")));
- ADD_COMMAND( "getGain2",
- makeDirectGetter(ent,&gain2,docDirectGetter("gain2","double")));
- }
+/* --- MULTIPLICATION --------------------------------------------------- */
- virtual std::string getDocString () const
- {
- return std::string("Weighted Combination of inputs : \n - gain{1|2} gain.");
- }
- };
+template <typename F, typename E>
+struct Multiplier_FxE__E : public BinaryOpHeader<F, E, E> {
+ void operator()(const F &f, const E &e, E &res) const { res = f * e; }
+};
- 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);
- }
+template <>
+void Multiplier_FxE__E<dynamicgraph::sot::MatrixHomogeneous,
+ dynamicgraph::Vector>::
+operator()(const dynamicgraph::sot::MatrixHomogeneous &f,
+ const dynamicgraph::Vector &e, dynamicgraph::Vector &res) const {
+ res = f.matrix() * e;
}
-#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)
+template <>
+void Multiplier_FxE__E<double, dynamicgraph::Vector>::
+operator()(const double &x, const dynamicgraph::Vector &v,
+ dynamicgraph::Vector &res) const {
+ res = v;
+ res *= x;
+}
-namespace dynamicgraph {
- namespace sot {
- template< typename Tin, typename Tout, typename Time >
- std::string VariadicAbstract<Tin,Tout,Time>::getTypeInName (void) { return TypeNameHelper<Tin>::typeName; }
- template< typename Tin, typename Tout, typename Time >
- std::string VariadicAbstract<Tin,Tout,Time>::getTypeOutName(void) { return TypeNameHelper<Tout>::typeName; }
-
- template< typename TypeIn, typename TypeOut >
- struct VariadicOpHeader
- {
- typedef TypeIn Tin ;
- typedef TypeOut Tout;
- static const std::string & nameTypeIn (void) { return TypeNameHelper<Tin >::typeName; }
- static const std::string & nameTypeOut(void) { return TypeNameHelper<Tout>::typeName; }
- template <typename Op>
- void initialize(VariadicOp<Op>*, Entity::CommandMap_t& ) {}
- virtual std::string getDocString () const
- {
- return std::string (
- "Undocumented variadic operator\n"
- " - input " + nameTypeIn () +
- "\n"
- " - output " + nameTypeOut () +
- "\n");
- }
- };
-
- /* --- VectorMix ------------------------------------------------------------ */
- struct VectorMix
- : public VariadicOpHeader<Vector,Vector>
- {
- public:
- typedef VariadicOp<VectorMix> Base;
- struct segment_t {
- Vector::Index index, size, input;
- std::size_t sigIdx;
- segment_t (Vector::Index i, Vector::Index s, std::size_t sig) : index (i), size(s), sigIdx (sig) {}
- };
- typedef std::vector <segment_t> segments_t;
- Base* entity;
- segments_t idxs;
- void operator()( const std::vector<const Vector*>& vs, Vector& res ) const
- {
- res = *vs[0];
- for (std::size_t i = 0; i < idxs.size(); ++i)
- {
- const segment_t& s = idxs[i];
- if (s.sigIdx >= vs.size())
- throw std::invalid_argument ("Index out of range in VectorMix");
- res.segment (s.index, s.size) = *vs[s.sigIdx];
- }
+typedef Multiplier_FxE__E<double, dynamicgraph::Vector>
+ Multiplier_double_vector;
+typedef Multiplier_FxE__E<dynamicgraph::Matrix, dynamicgraph::Vector>
+ Multiplier_matrix_vector;
+typedef Multiplier_FxE__E<MatrixHomogeneous, dynamicgraph::Vector>
+ Multiplier_matrixHomo_vector;
+REGISTER_BINARY_OP(Multiplier_double_vector, Multiply_double_vector);
+REGISTER_BINARY_OP(Multiplier_matrix_vector, Multiply_matrix_vector);
+REGISTER_BINARY_OP(Multiplier_matrixHomo_vector, Multiply_matrixHomo_vector);
+
+/* --- SUBSTRACTION ----------------------------------------------------- */
+template <typename T> struct Substraction : public BinaryOpHeader<T, T, T> {
+ void operator()(const T &v1, const T &v2, T &r) const {
+ r = v1;
+ r -= v2;
+ }
+};
+
+REGISTER_BINARY_OP(Substraction<dynamicgraph::Matrix>, Substract_of_matrix);
+REGISTER_BINARY_OP(Substraction<dynamicgraph::Vector>, Substract_of_vector);
+REGISTER_BINARY_OP(Substraction<double>, Substract_of_double);
+
+/* --- STACK ------------------------------------------------------------ */
+struct VectorStack
+ : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Vector,
+ dynamicgraph::Vector> {
+public:
+ int v1min, v1max;
+ int v2min, v2max;
+ void operator()(const dynamicgraph::Vector &v1,
+ const dynamicgraph::Vector &v2,
+ dynamicgraph::Vector &res) const {
+ assert((v1max >= v1min) && (v1.size() >= v1max));
+ assert((v2max >= v2min) && (v2.size() >= v2max));
+
+ const int v1size = v1max - v1min, v2size = v2max - v2min;
+ res.resize(v1size + v2size);
+ for (int i = 0; i < v1size; ++i) {
+ res(i) = v1(i + v1min);
+ }
+ for (int i = 0; i < v2size; ++i) {
+ res(v1size + i) = v2(i + v2min);
+ }
+ }
+
+ void selec1(const int &m, const int M) {
+ v1min = m;
+ v1max = M;
+ }
+ void selec2(const int &m, const int M) {
+ v2min = m;
+ v2max = M;
+ }
+
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ std::string doc;
+
+ boost::function<void(const int &, const int &)> selec1 =
+ boost::bind(&VectorStack::selec1, this, _1, _2);
+ boost::function<void(const int &, const int &)> selec2 =
+ boost::bind(&VectorStack::selec2, this, _1, _2);
+
+ ADD_COMMAND(
+ "selec1",
+ makeCommandVoid2(ent, selec1,
+ docCommandVoid2("set the min and max of selection.",
+ "int (imin)", "int (imax)")));
+ ADD_COMMAND(
+ "selec2",
+ makeCommandVoid2(ent, selec2,
+ docCommandVoid2("set the min and max of selection.",
+ "int (imin)", "int (imax)")));
+ }
+};
+REGISTER_BINARY_OP(VectorStack, Stack_of_vector);
+
+/* ---------------------------------------------------------------------- */
+
+struct Composer
+ : public BinaryOpHeader<dynamicgraph::Matrix, dynamicgraph::Vector,
+ MatrixHomogeneous> {
+ void operator()(const dynamicgraph::Matrix &R, const dynamicgraph::Vector &t,
+ MatrixHomogeneous &H) const {
+ H.linear() = R;
+ H.translation() = t;
+ }
+};
+REGISTER_BINARY_OP(Composer, Compose_R_and_T);
+
+/* --- CONVOLUTION PRODUCT ---------------------------------------------- */
+struct ConvolutionTemporal
+ : public BinaryOpHeader<dynamicgraph::Vector, dynamicgraph::Matrix,
+ dynamicgraph::Vector> {
+ typedef std::deque<dynamicgraph::Vector> MemoryType;
+ MemoryType memory;
+
+ void convolution(const MemoryType &f1, const dynamicgraph::Matrix &f2,
+ 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
+
+ res.resize(nsig);
+ res.fill(0);
+ unsigned int j = 0;
+ for (MemoryType::const_iterator iter = f1.begin(); iter != f1.end();
+ iter++) {
+ const dynamicgraph::Vector &s_tau = *iter;
+ sotDEBUG(45) << "Sig" << j << ": " << s_tau;
+ if (s_tau.size() != nsig)
+ return; // TODO: error throw;
+ for (int i = 0; i < nsig; ++i) {
+ res(i) += f2(i, j) * s_tau(i);
}
+ j++;
+ }
+ }
+ void operator()(const dynamicgraph::Vector &v1,
+ const dynamicgraph::Matrix &m2, dynamicgraph::Vector &res) {
+ memory.push_front(v1);
+ while ((Vector::Index)memory.size() > m2.cols())
+ memory.pop_back();
+ convolution(memory, m2, res);
+ }
+};
+REGISTER_BINARY_OP(ConvolutionTemporal, ConvolutionTemporal);
+
+/* --- BOOLEAN REDUCTION ------------------------------------------------ */
+
+template <typename T> struct Comparison : public BinaryOpHeader<T, T, bool> {
+ void operator()(const T &a, const T &b, bool &res) const { res = (a < b); }
+ virtual std::string getDocString() const {
+ typedef BinaryOpHeader<T, T, bool> Base;
+ return std::string("Comparison of inputs:\n"
+ " - input ") +
+ Base::nameTypeIn1() +
+ std::string("\n"
+ " - ") +
+ Base::nameTypeIn2() +
+ std::string("\n"
+ " - output ") +
+ Base::nameTypeOut() +
+ std::string("\n"
+ " sout = ( sin1 < sin2 )\n");
+ }
+};
+
+template <typename T1, typename T2 = T1>
+struct MatrixComparison : public BinaryOpHeader<T1, T2, bool> {
+ // TODO T1 or T2 could be a scalar type.
+ void operator()(const T1 &a, const T2 &b, bool &res) const {
+ if (equal && any)
+ res = (a.array() <= b.array()).any();
+ else if (equal && !any)
+ res = (a.array() <= b.array()).all();
+ else if (!equal && any)
+ res = (a.array() < b.array()).any();
+ else if (!equal && !any)
+ res = (a.array() < b.array()).all();
+ }
+ virtual std::string getDocString() const {
+ typedef BinaryOpHeader<T1, T2, bool> Base;
+ return std::string("Comparison of inputs:\n"
+ " - input ") +
+ Base::nameTypeIn1() +
+ std::string("\n"
+ " - ") +
+ Base::nameTypeIn2() +
+ 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");
+ }
+ MatrixComparison() : any(true), equal(false) {}
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ ADD_COMMAND(
+ "setTrueIfAny",
+ makeDirectSetter(ent, &any, docDirectSetter("trueIfAny", "bool")));
+ ADD_COMMAND(
+ "getTrueIfAny",
+ makeDirectGetter(ent, &any, docDirectGetter("trueIfAny", "bool")));
+ ADD_COMMAND("setEqual", makeDirectSetter(ent, &equal,
+ docDirectSetter("equal", "bool")));
+ ADD_COMMAND("getEqual", makeDirectGetter(ent, &equal,
+ docDirectGetter("equal", "bool")));
+ }
+ bool any, equal;
+};
- void addSelec( const int & sigIdx, const int & i, const int& s) { idxs.push_back(segment_t(i,s,sigIdx)); }
+REGISTER_BINARY_OP(Comparison<double>, CompareDouble);
+REGISTER_BINARY_OP(MatrixComparison<Vector>, CompareVector);
+} /* namespace sot */
+} /* namespace dynamicgraph */
- void initialize(Base* ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
- entity = ent;
+namespace dynamicgraph {
+namespace sot {
+
+template <typename T> struct WeightedAdder : public BinaryOpHeader<T, T, T> {
+public:
+ double gain1, gain2;
+ void operator()(const T &v1, const T &v2, T &res) const {
+ res = v1;
+ res *= gain1;
+ res += gain2 * v2;
+ }
- ent->addSignal ("default");
+ void addSpecificCommands(Entity &ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ std::string doc;
+
+ ADD_COMMAND(
+ "setGain1",
+ makeDirectSetter(ent, &gain1, docDirectSetter("gain1", "double")));
+ ADD_COMMAND(
+ "setGain2",
+ makeDirectSetter(ent, &gain2, docDirectSetter("gain2", "double")));
+ ADD_COMMAND(
+ "getGain1",
+ makeDirectGetter(ent, &gain1, docDirectGetter("gain1", "double")));
+ ADD_COMMAND(
+ "getGain2",
+ makeDirectGetter(ent, &gain2, docDirectGetter("gain2", "double")));
+ }
- boost::function< void( const int&, const int&, const int& ) > selec
- = boost::bind( &VectorMix::addSelec,this,_1,_2,_3 );
+ virtual std::string getDocString() const {
+ return std::string("Weighted Combination of inputs : \n - gain{1|2} gain.");
+ }
+};
+
+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);
+} // namespace sot
+} // namespace dynamicgraph
+
+#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)
- ADD_COMMAND( "setSignalNumber", makeCommandVoid1(*(typename Base::Base*)ent, &Base::setSignalNumber,
- docCommandVoid1("set the number of input vector.", "int (size)")));
+namespace dynamicgraph {
+namespace sot {
+template <typename Tin, typename Tout, typename Time>
+std::string VariadicAbstract<Tin, Tout, Time>::getTypeInName(void) {
+ return TypeNameHelper<Tin>::typeName;
+}
+template <typename Tin, typename Tout, typename Time>
+std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) {
+ return TypeNameHelper<Tout>::typeName;
+}
- commandMap.insert(std::make_pair( "getSignalNumber",
- new Getter<Base, int> (*ent, &Base::getSignalNumber,
- "Get the number of input vector.")));
+template <typename TypeIn, typename TypeOut> struct VariadicOpHeader {
+ typedef TypeIn Tin;
+ typedef TypeOut Tout;
+ static const std::string &nameTypeIn(void) {
+ return TypeNameHelper<Tin>::typeName;
+ }
+ static const std::string &nameTypeOut(void) {
+ return TypeNameHelper<Tout>::typeName;
+ }
+ template <typename Op>
+ void initialize(VariadicOp<Op> *, Entity::CommandMap_t &) {}
+ virtual std::string getDocString() const {
+ return std::string("Undocumented variadic operator\n"
+ " - input " +
+ nameTypeIn() +
+ "\n"
+ " - output " +
+ nameTypeOut() + "\n");
+ }
+};
+
+/* --- VectorMix ------------------------------------------------------------ */
+struct VectorMix : public VariadicOpHeader<Vector, Vector> {
+public:
+ typedef VariadicOp<VectorMix> Base;
+ struct segment_t {
+ Vector::Index index, size, input;
+ std::size_t sigIdx;
+ segment_t(Vector::Index i, Vector::Index s, std::size_t sig)
+ : index(i), size(s), sigIdx(sig) {}
+ };
+ typedef std::vector<segment_t> segments_t;
+ Base *entity;
+ segments_t idxs;
+ void operator()(const std::vector<const Vector *> &vs, Vector &res) const {
+ res = *vs[0];
+ for (std::size_t i = 0; i < idxs.size(); ++i) {
+ const segment_t &s = idxs[i];
+ if (s.sigIdx >= vs.size())
+ throw std::invalid_argument("Index out of range in VectorMix");
+ res.segment(s.index, s.size) = *vs[s.sigIdx];
+ }
+ }
- commandMap.insert(std::make_pair("addSelec",
- makeCommandVoid3<Base,int,int,int>(*ent, selec,
- docCommandVoid3("add selection from a vector.",
- "int (signal index >= 1)", "int (index)","int (size)"))));
- }
- };
- REGISTER_VARIADIC_OP(VectorMix,Mix_of_vector);
-
- /* --- ADDITION --------------------------------------------------------- */
- template< typename T>
- struct AdderVariadic
- : public VariadicOpHeader<T,T>
- {
- typedef VariadicOp<AdderVariadic> Base;
-
- Base* entity;
- Vector coeffs;
-
- AdderVariadic () : coeffs () {}
- void operator()( const std::vector<const T*>& vs, T& res ) const
- {
- assert (vs.size() == (std::size_t)coeffs.size());
- 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]);
- }
+ void addSelec(const int &sigIdx, const int &i, const int &s) {
+ idxs.push_back(segment_t(i, s, sigIdx));
+ }
- void setCoeffs(const Vector& c) { coeffs = c; }
- void setCoeff1(const double& c) { assert(coeffs.size() == 2); coeffs(0) = c; }
- void setCoeff2(const double& c) { assert(coeffs.size() == 2); coeffs(1) = c; }
-
- void setSignalNumber (const int& n)
- {
- if (entity->getSignalNumber() == 2) {
- entity->removeSignal();
- entity->removeSignal();
- }
- coeffs = Vector::Ones(n);
- entity->setSignalNumber(n);
- }
+ void initialize(Base *ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ entity = ent;
- void initialize(Base* ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
- entity = ent;
-
- coeffs = Vector::Ones(2);
- entity->addSignal ("sin1");
- entity->addSignal ("sin2");
-
- commandMap.insert(std::make_pair( "getSignalNumber",
- new Getter<Base, int> (*ent, &Base::getSignalNumber,
- "Get the number of input vector.")));
-
- commandMap.insert(std::make_pair("setSignalNumber",
- makeCommandVoid1<Base,int>(*ent,
- boost::function <void (const int&)> (boost::bind ( &AdderVariadic::setSignalNumber, this, _1)),
- docCommandVoid1("set the number of input vector.", "int (size)"))));
-
- commandMap.insert(std::make_pair("setCoeffs",
- makeCommandVoid1<Base,Vector>(*ent,
- boost::function <void (const Vector&)> (boost::bind ( &AdderVariadic::setCoeffs, this, _1)),
- docCommandVoid1("set the multipliers.", "vector"))));
-
- // Add deprecated commands.
- commandMap.insert(std::make_pair("setCoeff1",
- makeCommandVoid1<Base,double>(*ent,
- boost::function <void (const double&)> (boost::bind ( &AdderVariadic::setCoeff1, this, _1)),
- docCommandVoid1("deprecated. Use setCoeffs.", "double"))));
-
- commandMap.insert(std::make_pair("setCoeff2",
- makeCommandVoid1<Base,double>(*ent,
- boost::function <void (const double&)> (boost::bind ( &AdderVariadic::setCoeff2, this, _1)),
- docCommandVoid1("deprecated. Use setCoeffs.", "double"))));
- }
+ ent->addSignal("default");
- virtual std::string getDocString () const
- {
- return
- "Linear combination of inputs\n"
- " - input " + VariadicOpHeader<T,T>::nameTypeIn () +
- "\n"
- " - output " + VariadicOpHeader<T,T>::nameTypeOut () +
- "\n"
- " sout = sum ([coeffs[i] * sin[i] for i in range(n) ])\n"
- " Coefficients are set by commands, default value is 1.\n";
- }
- };
- REGISTER_VARIADIC_OP(AdderVariadic<Matrix>,Add_of_matrix);
- REGISTER_VARIADIC_OP(AdderVariadic<Vector>,Add_of_vector);
- REGISTER_VARIADIC_OP(AdderVariadic<double>,Add_of_double);
-
- /* --- MULTIPLICATION --------------------------------------------------- */
- template< typename T>
- struct Multiplier
- : public VariadicOpHeader<T,T>
- {
- typedef VariadicOp<Multiplier> Base;
-
- void operator()( const std::vector<const T*>& vs,T& res ) const
- {
- if (vs.size() == 0) setIdentity(res);
- else {
- res = *vs[0];
- for (std::size_t i = 1; i < vs.size(); ++i) res *= *vs[i];
- }
- }
+ boost::function<void(const int &, const int &, const int &)> selec =
+ boost::bind(&VectorMix::addSelec, this, _1, _2, _3);
- void setIdentity (T& res) const { res.setIdentity(); }
+ ADD_COMMAND(
+ "setSignalNumber",
+ makeCommandVoid1(
+ *(typename Base::Base *)ent, &Base::setSignalNumber,
+ docCommandVoid1("set the number of input vector.", "int (size)")));
- void initialize(Base* ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
+ commandMap.insert(std::make_pair(
+ "getSignalNumber",
+ new Getter<Base, int>(*ent, &Base::getSignalNumber,
+ "Get the number of input vector.")));
- ent->setSignalNumber (2);
+ commandMap.insert(std::make_pair(
+ "addSelec", makeCommandVoid3<Base, int, int, int>(
+ *ent, selec,
+ docCommandVoid3("add selection from a vector.",
+ "int (signal index >= 1)",
+ "int (index)", "int (size)"))));
+ }
+};
+REGISTER_VARIADIC_OP(VectorMix, Mix_of_vector);
+
+/* --- ADDITION --------------------------------------------------------- */
+template <typename T> struct AdderVariadic : public VariadicOpHeader<T, T> {
+ typedef VariadicOp<AdderVariadic> Base;
+
+ Base *entity;
+ Vector coeffs;
+
+ AdderVariadic() : coeffs() {}
+ void operator()(const std::vector<const T *> &vs, T &res) const {
+ assert(vs.size() == (std::size_t)coeffs.size());
+ 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]);
+ }
- ADD_COMMAND( "setSignalNumber", makeCommandVoid1(*(typename Base::Base*)ent, &Base::setSignalNumber,
- docCommandVoid1("set the number of input vector.", "int (size)")));
+ void setCoeffs(const Vector &c) { coeffs = c; }
+ void setCoeff1(const double &c) {
+ assert(coeffs.size() == 2);
+ coeffs(0) = c;
+ }
+ void setCoeff2(const double &c) {
+ assert(coeffs.size() == 2);
+ coeffs(1) = c;
+ }
- commandMap.insert(std::make_pair( "getSignalNumber",
- new Getter<Base, int> (*ent, &Base::getSignalNumber,
- "Get the number of input vector.")));
- }
- };
- template<> void Multiplier<double>::
- setIdentity (double& res) const { res = 1; }
- template<> 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];
- }
+ void setSignalNumber(const int &n) {
+ if (entity->getSignalNumber() == 2) {
+ entity->removeSignal();
+ entity->removeSignal();
}
- template<> 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();
- }
+ coeffs = Vector::Ones(n);
+ entity->setSignalNumber(n);
+ }
+
+ void initialize(Base *ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+ entity = ent;
+
+ coeffs = Vector::Ones(2);
+ entity->addSignal("sin1");
+ entity->addSignal("sin2");
+
+ commandMap.insert(std::make_pair(
+ "getSignalNumber",
+ new Getter<Base, int>(*ent, &Base::getSignalNumber,
+ "Get the number of input vector.")));
+
+ commandMap.insert(std::make_pair(
+ "setSignalNumber",
+ makeCommandVoid1<Base, int>(
+ *ent,
+ boost::function<void(const int &)>(
+ boost::bind(&AdderVariadic::setSignalNumber, this, _1)),
+ docCommandVoid1("set the number of input vector.", "int (size)"))));
+
+ commandMap.insert(std::make_pair(
+ "setCoeffs", makeCommandVoid1<Base, Vector>(
+ *ent,
+ boost::function<void(const Vector &)>(
+ boost::bind(&AdderVariadic::setCoeffs, this, _1)),
+ docCommandVoid1("set the multipliers.", "vector"))));
+
+ // Add deprecated commands.
+ commandMap.insert(std::make_pair(
+ "setCoeff1",
+ makeCommandVoid1<Base, double>(
+ *ent,
+ boost::function<void(const double &)>(
+ boost::bind(&AdderVariadic::setCoeff1, this, _1)),
+ docCommandVoid1("deprecated. Use setCoeffs.", "double"))));
+
+ commandMap.insert(std::make_pair(
+ "setCoeff2",
+ makeCommandVoid1<Base, double>(
+ *ent,
+ boost::function<void(const double &)>(
+ boost::bind(&AdderVariadic::setCoeff2, this, _1)),
+ docCommandVoid1("deprecated. Use setCoeffs.", "double"))));
+ }
+
+ virtual std::string getDocString() const {
+ return "Linear combination of inputs\n"
+ " - input " +
+ VariadicOpHeader<T, T>::nameTypeIn() +
+ "\n"
+ " - output " +
+ VariadicOpHeader<T, T>::nameTypeOut() +
+ "\n"
+ " sout = sum ([coeffs[i] * sin[i] for i in range(n) ])\n"
+ " Coefficients are set by commands, default value is 1.\n";
+ }
+};
+REGISTER_VARIADIC_OP(AdderVariadic<Matrix>, Add_of_matrix);
+REGISTER_VARIADIC_OP(AdderVariadic<Vector>, Add_of_vector);
+REGISTER_VARIADIC_OP(AdderVariadic<double>, Add_of_double);
+
+/* --- MULTIPLICATION --------------------------------------------------- */
+template <typename T> struct Multiplier : public VariadicOpHeader<T, T> {
+ typedef VariadicOp<Multiplier> Base;
+
+ void operator()(const std::vector<const T *> &vs, T &res) const {
+ if (vs.size() == 0)
+ setIdentity(res);
+ else {
+ res = *vs[0];
+ for (std::size_t i = 1; i < vs.size(); ++i)
+ res *= *vs[i];
}
+ }
- REGISTER_VARIADIC_OP(Multiplier<Matrix >,Multiply_of_matrix);
- REGISTER_VARIADIC_OP(Multiplier<Vector >,Multiply_of_vector);
- REGISTER_VARIADIC_OP(Multiplier<MatrixRotation >,Multiply_of_matrixrotation);
- REGISTER_VARIADIC_OP(Multiplier<MatrixHomogeneous>,Multiply_of_matrixHomo);
- REGISTER_VARIADIC_OP(Multiplier<MatrixTwist >,Multiply_of_matrixtwist);
- REGISTER_VARIADIC_OP(Multiplier<VectorQuaternion >,Multiply_of_quaternion);
- REGISTER_VARIADIC_OP(Multiplier<double >,Multiply_of_double);
-
- /* --- BOOLEAN --------------------------------------------------------- */
- template <int operation>
- struct BoolOp
- : public VariadicOpHeader<bool,bool>
- {
- typedef VariadicOp<BoolOp> Base;
-
- 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;
- 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;
- }
- }
+ void setIdentity(T &res) const { res.setIdentity(); }
- void initialize(Base* ent,
- Entity::CommandMap_t& commandMap )
- {
- using namespace dynamicgraph::command;
+ void initialize(Base *ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
- ADD_COMMAND( "setSignalNumber", makeCommandVoid1(*(typename Base::Base*)ent, &Base::setSignalNumber,
- docCommandVoid1("set the number of input boolean.", "int (size)")));
+ ent->setSignalNumber(2);
- commandMap.insert(std::make_pair( "getSignalNumber",
- new Getter<Base, int> (*ent, &Base::getSignalNumber,
- "Get the number of input bool.")));
- }
- };
- REGISTER_VARIADIC_OP(BoolOp<0>,And);
- REGISTER_VARIADIC_OP(BoolOp<1>,Or);
+ ADD_COMMAND(
+ "setSignalNumber",
+ makeCommandVoid1(
+ *(typename Base::Base *)ent, &Base::setSignalNumber,
+ docCommandVoid1("set the number of input vector.", "int (size)")));
+ commandMap.insert(std::make_pair(
+ "getSignalNumber",
+ new Getter<Base, int>(*ent, &Base::getSignalNumber,
+ "Get the number of input vector.")));
+ }
+};
+template <> void Multiplier<double>::setIdentity(double &res) const { res = 1; }
+template <>
+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];
+ }
+}
+template <>
+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();
}
}
+REGISTER_VARIADIC_OP(Multiplier<Matrix>, Multiply_of_matrix);
+REGISTER_VARIADIC_OP(Multiplier<Vector>, Multiply_of_vector);
+REGISTER_VARIADIC_OP(Multiplier<MatrixRotation>, Multiply_of_matrixrotation);
+REGISTER_VARIADIC_OP(Multiplier<MatrixHomogeneous>, Multiply_of_matrixHomo);
+REGISTER_VARIADIC_OP(Multiplier<MatrixTwist>, Multiply_of_matrixtwist);
+REGISTER_VARIADIC_OP(Multiplier<VectorQuaternion>, Multiply_of_quaternion);
+REGISTER_VARIADIC_OP(Multiplier<double>, Multiply_of_double);
+
+/* --- BOOLEAN --------------------------------------------------------- */
+template <int operation> struct BoolOp : public VariadicOpHeader<bool, bool> {
+ typedef VariadicOp<BoolOp> Base;
+
+ 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;
+ 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;
+ }
+ }
+
+ void initialize(Base *ent, Entity::CommandMap_t &commandMap) {
+ using namespace dynamicgraph::command;
+
+ ADD_COMMAND(
+ "setSignalNumber",
+ makeCommandVoid1(
+ *(typename Base::Base *)ent, &Base::setSignalNumber,
+ docCommandVoid1("set the number of input boolean.", "int (size)")));
+
+ commandMap.insert(
+ std::make_pair("getSignalNumber",
+ new Getter<Base, int>(*ent, &Base::getSignalNumber,
+ "Get the number of input bool.")));
+ }
+};
+REGISTER_VARIADIC_OP(BoolOp<0>, And);
+REGISTER_VARIADIC_OP(BoolOp<1>, Or);
+
+} // namespace sot
+} // namespace dynamicgraph
/* --- TODO ------------------------------------------------------------------*/
// The following commented lines are sot-v1 entities that are still waiting
// for conversion. Help yourself!
-// /* -------------------------------------------------------------------------- */
+// /* --------------------------------------------------------------------------
+// */
// struct WeightedDirection
// {
// public:
-// void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& v2,dynamicgraph::Vector& res ) const
+// void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
+// v2,dynamicgraph::Vector& res ) const
// {
// const double norm1 = v1.norm();
// const double norm2 = v2.norm();
@@ -1169,13 +1146,14 @@ namespace dynamicgraph {
// typedef BinaryOp< Vector,Vector,Vector,WeightedDirection > weightdir;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(weightdir,vector,weight_dir,"WeightDir")
-
-// /* -------------------------------------------------------------------------- */
+// /* --------------------------------------------------------------------------
+// */
// struct Nullificator
// {
// public:
-// void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& v2,dynamicgraph::Vector& res ) const
+// void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
+// v2,dynamicgraph::Vector& res ) const
// {
// const unsigned int s = std::max( v1.size(),v2.size() );
// res.resize(s);
@@ -1190,16 +1168,16 @@ namespace dynamicgraph {
// typedef BinaryOp< Vector,Vector,Vector,Nullificator > vectNil;
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(vectNil,vector,vectnil_,"Nullificator")
-
-
-// /* -------------------------------------------------------------------------- */
+// /* --------------------------------------------------------------------------
+// */
// struct VirtualSpring
// {
// public:
// double spring;
-// void operator()( const dynamicgraph::Vector& pos,const dynamicgraph::Vector& ref,dynamicgraph::Vector& res ) const
+// void operator()( const dynamicgraph::Vector& pos,const
+// dynamicgraph::Vector& ref,dynamicgraph::Vector& res ) const
// {
// double norm = ref.norm();
// double dist = ref.scalarProduct(pos) / (norm*norm);
diff --git a/src/matrix/vector-constant-command.h b/src/matrix/vector-constant-command.h
index 6715eeda..f6844559 100644
--- a/src/matrix/vector-constant-command.h
+++ b/src/matrix/vector-constant-command.h
@@ -11,43 +11,41 @@
#include <boost/assign/list_of.hpp>
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command.h>
-namespace dynamicgraph { namespace sot {
- namespace command {
- namespace vectorConstant {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
+namespace dynamicgraph {
+namespace sot {
+namespace command {
+namespace vectorConstant {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
- // Command Resize
- class Resize : public Command
- {
- public:
- virtual ~Resize() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Resize(VectorConstant& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring)
- {
- }
- virtual Value doExecute()
- {
- VectorConstant& vc = static_cast<VectorConstant&>(owner());
- std::vector<Value> values = getParameterValues();
- unsigned size = values[0].value();
- dynamicgraph::Vector m(size);
- m.fill(vc.color);
- vc.SOUT.setConstant(m);
+// Command Resize
+class Resize : public Command {
+public:
+ virtual ~Resize() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Resize(VectorConstant &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring) {}
+ virtual Value doExecute() {
+ VectorConstant &vc = static_cast<VectorConstant &>(owner());
+ std::vector<Value> values = getParameterValues();
+ unsigned size = values[0].value();
+ dynamicgraph::Vector m(size);
+ m.fill(vc.color);
+ vc.SOUT.setConstant(m);
- // return void
- return Value();
- }
- }; // class Resize
- } //namespace vectorConstant
- } // namespace command
-} /* namespace sot */} /* namespace dynamicgraph */
+ // return void
+ return Value();
+ }
+}; // 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 29405561..d57ba43b 100644
--- a/src/matrix/vector-constant.cpp
+++ b/src/matrix/vector-constant.cpp
@@ -7,8 +7,8 @@
*
*/
-#include <sot/core/vector-constant.hh>
#include <sot/core/factory.hh>
+#include <sot/core/vector-constant.hh>
#include "../src/matrix/vector-constant-command.h"
@@ -16,48 +16,42 @@ using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorConstant,"VectorConstant");
-
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorConstant, "VectorConstant");
/* --------------------------------------------------------------------- */
/* --- VECTOR ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-VectorConstant::
-VectorConstant( const std::string& name )
- :Entity( name )
- ,rows(0),color(0.)
- ,SOUT( "sotVectorConstant("+name+")::output(vector)::sout" )
-{
- SOUT.setDependencyType( TimeDependency<int>::BOOL_DEPENDENT );
- signalRegistration( SOUT );
+VectorConstant::VectorConstant(const std::string &name)
+ : Entity(name), rows(0), color(0.),
+ SOUT("sotVectorConstant(" + name + ")::output(vector)::sout") {
+ SOUT.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
+ signalRegistration(SOUT);
//
// Commands
//
// Resize
std::string docstring;
- docstring = " \n"
- " Resize the vector and fill with value stored in color field.\n"
- " Input\n"
- " unsigned size.\n"
- "\n";
- addCommand("resize",
- new command::vectorConstant::Resize(*this, docstring));
+ docstring =
+ " \n"
+ " Resize the vector and fill with value stored in color field.\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";
- addCommand("set",
- new ::dynamicgraph::command::Setter<VectorConstant, dynamicgraph::Vector>
- (*this, &VectorConstant::setValue, docstring));
+ " Set value of output signal\n"
+ " \n"
+ " input:\n"
+ " - a vector\n"
+ " \n";
+ addCommand(
+ "set",
+ new ::dynamicgraph::command::Setter<VectorConstant, dynamicgraph::Vector>(
+ *this, &VectorConstant::setValue, docstring));
}
-void VectorConstant::
-setValue(const dynamicgraph::Vector& inValue)
-{
+void VectorConstant::setValue(const dynamicgraph::Vector &inValue) {
SOUT.setConstant(inValue);
}
diff --git a/src/matrix/vector-to-rotation.cpp b/src/matrix/vector-to-rotation.cpp
index 4fd351fb..5dcd641d 100644
--- a/src/matrix/vector-to-rotation.cpp
+++ b/src/matrix/vector-to-rotation.cpp
@@ -9,74 +9,71 @@
#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/debug.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorToRotation,"VectorToRotation");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorToRotation, "VectorToRotation");
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-VectorToRotation::
-VectorToRotation( const std::string& name )
- :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" )
-{
+VectorToRotation::VectorToRotation(const std::string &name)
+ : 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 );
+ 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 )
- {
- Ra.setIdentity();
- const double ca = cos( angles(i) );
- 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;
- }
- }
-
- sotDEBUG(15) << "R" << i << " = " << Ra;
- Rtmp = res*Ra;
- res=Rtmp;
+ MatrixRotation Ra, Rtmp;
+ for (unsigned int i = 0; i < size; ++i) {
+ Ra.setIdentity();
+ const double ca = cos(angles(i));
+ 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;
+ }
+ }
+
+ sotDEBUG(15) << "R" << i << " = " << Ra;
+ Rtmp = res * Ra;
+ res = Rtmp;
+ }
return res;
}
diff --git a/src/signal/signal-cast.cpp b/src/signal/signal-cast.cpp
index c5647349..4db6cd32 100644
--- a/src/signal/signal-cast.cpp
+++ b/src/signal/signal-cast.cpp
@@ -7,232 +7,222 @@
*
*/
-#include <sot/core/matrix-geometry.hh>
-#include <dynamic-graph/signal-caster.h>
-#include <dynamic-graph/eigen-io.h>
#include <Eigen/Core>
-#include <sot/core/pool.hh>
+#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 <sot/core/feature-abstract.hh>
-#include <sot/core/trajectory.hh>
#include <sot/core/flags.hh>
#include <sot/core/multi-bound.hh>
-#include <dynamic-graph/signal-caster.h>
-#include <dynamic-graph/signal-cast-helper.h>
+#include <sot/core/trajectory.hh>
-# ifdef WIN32
-# include <Windows.h>
-# endif
+#ifdef WIN32
+#include <Windows.h>
+#endif
/* Implements a set of caster/displayer for the main types of sot-core. */
-
-namespace dynamicgraph
-{
- using namespace std;
- using namespace dynamicgraph::sot;
- namespace dgsot = dynamicgraph::sot;
-
- /* --- CASTER IMPLEMENTATION ------------------------------------------------ */
- /* --- CASTER IMPLEMENTATION ------------------------------------------------ */
- /* --- CASTER IMPLEMENTATION ------------------------------------------------ */
-
- DG_SIGNAL_CAST_DEFINITION(sot::Flags);
- DG_ADD_CASTER(sot::Flags,flags);
-
-
- template <>
- void
- DefaultCastRegisterer<MatrixHomogeneous>::
- trace(const boost::any& object, std::ostream& os)
- {
- const MatrixHomogeneous & M = boost::any_cast<MatrixHomogeneous>(object);
- for( unsigned int i=0;i<4;++i )
- for( unsigned int j=0;j<4;++j )
- { os << "\t" << M(i,j); }
- }
- namespace {
- dynamicgraph::DefaultCastRegisterer <sot::MatrixHomogeneous>
+namespace dynamicgraph {
+using namespace std;
+using namespace dynamicgraph::sot;
+namespace dgsot = dynamicgraph::sot;
+
+/* --- CASTER IMPLEMENTATION ------------------------------------------------ */
+/* --- CASTER IMPLEMENTATION ------------------------------------------------ */
+/* --- CASTER IMPLEMENTATION ------------------------------------------------ */
+
+DG_SIGNAL_CAST_DEFINITION(sot::Flags);
+DG_ADD_CASTER(sot::Flags, flags);
+
+template <>
+void DefaultCastRegisterer<MatrixHomogeneous>::trace(const boost::any &object,
+ std::ostream &os) {
+ const MatrixHomogeneous &M = boost::any_cast<MatrixHomogeneous>(object);
+ for (unsigned int i = 0; i < 4; ++i)
+ for (unsigned int j = 0; j < 4; ++j) {
+ os << "\t" << M(i, j);
+ }
+}
+namespace {
+dynamicgraph::DefaultCastRegisterer<sot::MatrixHomogeneous>
matrixHomegeneousCastRegisterer;
- dynamicgraph::DefaultCastRegisterer <sot::MatrixRotation>
+dynamicgraph::DefaultCastRegisterer<sot::MatrixRotation>
matrixRotationCastRegisterer;
+} // namespace
+/* --- FEATURE ABSTRACT ----------------------------------------------------- */
+/* --- FEATURE ABSTRACT ----------------------------------------------------- */
+/* --- FEATURE ABSTRACT ----------------------------------------------------- */
+
+DG_SIGNAL_CAST_DEFINITION_HPP(sot::FeatureAbstract *);
+
+FeatureAbstract *SignalCast<FeatureAbstract *>::cast(std::istringstream &iss) {
+ FeatureAbstract *ref;
+ std::string name;
+ iss >> name;
+ if (name.length())
+ ref = &dynamicgraph::sot::PoolStorage::getInstance()->getFeature(name);
+ else
+ ref = 0;
+ return ref;
+}
+
+void SignalCast<FeatureAbstract *>::disp(FeatureAbstract *const &t,
+ std::ostream &os) {
+ if (t) {
+ t->display(os);
+ os << std::endl;
+ } else {
+ os << "NULL" << std::endl;
}
- /* --- FEATURE ABSTRACT ----------------------------------------------------- */
- /* --- FEATURE ABSTRACT ----------------------------------------------------- */
- /* --- FEATURE ABSTRACT ----------------------------------------------------- */
-
- DG_SIGNAL_CAST_DEFINITION_HPP(sot::FeatureAbstract*);
-
- FeatureAbstract* SignalCast<FeatureAbstract*>::
- cast( std::istringstream& iss )
- {
- FeatureAbstract* ref;
- std::string name; iss >> name;
- if( name.length())
- ref = &dynamicgraph::sot::PoolStorage::getInstance()->getFeature(name);
- else ref = 0;
- return ref;
- }
-
- void SignalCast<FeatureAbstract*>::
- disp( FeatureAbstract* const& t,std::ostream& os )
- {
- if( t ) { t->display(os); os<<std::endl; }
- else { os << "NULL" << std::endl; }
- }
-
- DG_ADD_CASTER(FeatureAbstract*,FAptr);
-
-
- /* --- TIMEVAL -------------------------------------------------------------- */
- /* --- TIMEVAL -------------------------------------------------------------- */
- /* --- TIMEVAL -------------------------------------------------------------- */
- DG_SIGNAL_CAST_DEFINITION_HPP(struct timeval);
-
- struct timeval SignalCast<struct timeval>::
- cast( std::istringstream& iss )
- {
- int u,s; iss >> s >> u;
- struct timeval t; t.tv_sec = s; t.tv_usec = u;
- return t;
+}
+
+DG_ADD_CASTER(FeatureAbstract *, FAptr);
+
+/* --- TIMEVAL -------------------------------------------------------------- */
+/* --- TIMEVAL -------------------------------------------------------------- */
+/* --- TIMEVAL -------------------------------------------------------------- */
+DG_SIGNAL_CAST_DEFINITION_HPP(struct timeval);
+
+struct timeval SignalCast<struct timeval>::cast(std::istringstream &iss) {
+ int u, s;
+ iss >> s >> u;
+ struct timeval t;
+ t.tv_sec = s;
+ t.tv_usec = u;
+ return t;
+} void SignalCast<struct timeval>::disp(const struct timeval &t,
+ std::ostream &os) {
+ os << t.tv_sec << "s " << t.tv_usec << "ms";
+}
+
+DG_ADD_CASTER(struct timeval, tv);
+
+/* --- Trajectory --------------------------------------------------------------
+ */
+/* --- Trajectory --------------------------------------------------------------
+ */
+/* --- Trajectory --------------------------------------------------------------
+ */
+DG_SIGNAL_CAST_DEFINITION_HPP(dgsot::Trajectory);
+
+dgsot::Trajectory SignalCast<dgsot::Trajectory>::cast(std::istringstream &iss) {
+ dgsot::Trajectory aTraj;
+
+ // Read joint names.
+ std::vector<std::string>::size_type nb_joints;
+ iss >> nb_joints;
+ aTraj.joint_names_.resize(nb_joints);
+ for (std::vector<std::string>::size_type idJoints = 0; idJoints < nb_joints;
+ idJoints++)
+ iss >> aTraj.joint_names_[idJoints];
+
+ // Read nb of points
+ std::vector<JointTrajectoryPoint>::size_type nb_points;
+ iss >> nb_points;
+
+ // Read points
+ for (std::vector<JointTrajectoryPoint>::size_type idPoint = 0;
+ idPoint < nb_points; idPoint++) {
+ // Read positions.
+ for (std::vector<double>::size_type idPos = 0; idPos < nb_joints; idPos++)
+ iss >> aTraj.points_[idPoint].positions_[idPos];
+ // TODO: read velocities and accelerations.
}
- void SignalCast<struct timeval>::
- disp( const struct timeval& t,std::ostream& os )
- {
- os << t.tv_sec << "s "<< t.tv_usec << "ms";
+ return aTraj;
+}
+void SignalCast<dgsot::Trajectory>::disp(const dgsot::Trajectory &aTraj,
+ std::ostream &os) {
+ // Display joint names.
+ os << "{ Number of joints: " << aTraj.joint_names_.size() << std::endl;
+ for (std::vector<std::string>::size_type idJoints = 0;
+ idJoints < aTraj.joint_names_.size(); idJoints++) {
+ os << idJoints << " - " << aTraj.joint_names_[idJoints] << std::endl;
}
-
- DG_ADD_CASTER(struct timeval,tv);
-
- /* --- Trajectory -------------------------------------------------------------- */
- /* --- Trajectory -------------------------------------------------------------- */
- /* --- Trajectory -------------------------------------------------------------- */
- DG_SIGNAL_CAST_DEFINITION_HPP(dgsot::Trajectory);
-
- dgsot::Trajectory SignalCast<dgsot::Trajectory>::
- cast( std::istringstream& iss )
- {
- dgsot::Trajectory aTraj;
-
- // Read joint names.
- std::vector<std::string>::size_type nb_joints;
- iss >> nb_joints; aTraj.joint_names_.resize(nb_joints);
- for(std::vector<std::string>::size_type idJoints=0;
- idJoints < nb_joints; idJoints++)
- iss >> aTraj.joint_names_[idJoints];
-
- // Read nb of points
- std::vector<JointTrajectoryPoint>::size_type nb_points;
- iss >> nb_points;
-
- // Read points
- for(std::vector<JointTrajectoryPoint>::size_type idPoint=0;
- idPoint < nb_points; idPoint++)
- {
- // Read positions.
- for(std::vector<double>::size_type idPos=0;
- idPos < nb_joints; idPos++)
- iss >> aTraj.points_[idPoint].positions_[idPos];
- // TODO: read velocities and accelerations.
+ // Display points
+ os << "Number of points: " << aTraj.points_.size() << std::endl;
+ for (std::vector<JointTrajectoryPoint>::size_type idPoint = 0;
+ idPoint < aTraj.points_.size(); idPoint++) {
+ if (aTraj.points_[idPoint].positions_.size() != 0) {
+ os << " Point " << idPoint << " - Pos: [";
+ // Read positions.
+ for (std::vector<double>::size_type idPos = 0;
+ idPos < aTraj.points_[idPoint].positions_.size(); idPos++) {
+ os << "(" << idPos << " : " << aTraj.points_[idPoint].positions_[idPos]
+ << ") ";
}
- return aTraj;
- }
- void SignalCast<dgsot::Trajectory >::
- disp( const dgsot::Trajectory & aTraj,std::ostream& os )
- {
- // Display joint names.
- os << "{ Number of joints: " << aTraj.joint_names_.size() << std::endl;
- for(std::vector<std::string>::size_type idJoints=0;
- idJoints < aTraj.joint_names_.size(); idJoints++)
- {
- os << idJoints << " - "
- << aTraj.joint_names_[idJoints]
- << std::endl;
+ os << "] ";
+ }
+ if (aTraj.points_[idPoint].velocities_.size() != 0) {
+ os << " Velocities " << idPoint << " - Pos: [";
+ // Read positions.
+ for (std::vector<double>::size_type idPos = 0;
+ idPos < aTraj.points_[idPoint].velocities_.size(); idPos++) {
+ os << "(" << idPos << " : " << aTraj.points_[idPoint].velocities_[idPos]
+ << ") ";
}
- // Display points
- os << "Number of points: "
- << aTraj.points_.size()
- << std::endl;
- for(std::vector<JointTrajectoryPoint>::size_type idPoint=0;
- idPoint < aTraj.points_.size(); idPoint++)
- {
- if (aTraj.points_[idPoint].positions_.size()!=0)
- {
- os << " Point " << idPoint << " - Pos: [" ;
- // Read positions.
- for(std::vector<double>::size_type idPos=0;
- idPos < aTraj.points_[idPoint].positions_.size(); idPos++)
- { os << "(" << idPos << " : " << aTraj.points_[idPoint].positions_[idPos] << ") " ; }
- os << "] ";
- }
- if (aTraj.points_[idPoint].velocities_.size()!=0)
- {
- os << " Velocities " << idPoint << " - Pos: [" ;
- // Read positions.
- for(std::vector<double>::size_type idPos=0;
- idPos < aTraj.points_[idPoint].velocities_.size(); idPos++)
- { os << "(" << idPos << " : " << aTraj.points_[idPoint].velocities_[idPos] << ") " ; }
- os << "] ";
- }
- if (aTraj.points_[idPoint].accelerations_.size()!=0)
- {
- os << " Velocities " << idPoint << " - Pos: [" ;
- // Read positions.
- for(std::vector<double>::size_type idPos=0;
- idPos < aTraj.points_[idPoint].accelerations_.size(); idPos++)
- { os << "(" << idPos << " : " << aTraj.points_[idPoint].accelerations_[idPos] << ") " ; }
- os << "] ";
- }
-
- // TODO: read velocities and accelerations.
+ os << "] ";
+ }
+ if (aTraj.points_[idPoint].accelerations_.size() != 0) {
+ os << " Velocities " << idPoint << " - Pos: [";
+ // Read positions.
+ for (std::vector<double>::size_type idPos = 0;
+ idPos < aTraj.points_[idPoint].accelerations_.size(); idPos++) {
+ os << "(" << idPos << " : "
+ << aTraj.points_[idPoint].accelerations_[idPos] << ") ";
}
- os << "}" << std::endl;
+ os << "] ";
+ }
+
+ // TODO: read velocities and accelerations.
}
+ os << "}" << std::endl;
+}
- DG_ADD_CASTER(Trajectory,Traject);
+DG_ADD_CASTER(Trajectory, Traject);
- ///
- /// VectorUTheta and VectorRollPitchYaw
- ///
- namespace {
- dynamicgraph::DefaultCastRegisterer <VectorUTheta>
- vectorUThetaCastRegisterer;
+///
+/// VectorUTheta and VectorRollPitchYaw
+///
+namespace {
+dynamicgraph::DefaultCastRegisterer<VectorUTheta> vectorUThetaCastRegisterer;
- dynamicgraph::DefaultCastRegisterer <VectorRollPitchYaw>
+dynamicgraph::DefaultCastRegisterer<VectorRollPitchYaw>
vectorRollPitchYawCastRegisterer;
+} // namespace
+
+/* --- MULTI BOUND ---------------------------------------------------------- */
+/* --- MULTI BOUND ---------------------------------------------------------- */
+/* --- MULTI BOUND ---------------------------------------------------------- */
+
+DG_SIGNAL_CAST_DEFINITION_TRACE(sot::VectorMultiBound);
+
+void SignalCast<VectorMultiBound>::trace(const VectorMultiBound &t,
+ std::ostream &os) {
+ for (VectorMultiBound::const_iterator iter = t.begin(); t.end() != iter;
+ ++iter) {
+ switch (iter->mode) {
+ case MultiBound::MODE_SINGLE:
+ os << iter->getSingleBound() << "\t";
+ break;
+ case MultiBound::MODE_DOUBLE:
+ if (iter->getDoubleBoundSetup(MultiBound::BOUND_INF))
+ os << iter->getDoubleBound(MultiBound::BOUND_INF) << "\t";
+ else
+ os << "-inf\t";
+ if (iter->getDoubleBoundSetup(MultiBound::BOUND_SUP))
+ os << iter->getDoubleBound(MultiBound::BOUND_SUP) << "\t";
+ else
+ os << "+inf\t";
+ break;
+ }
}
+}
+DG_ADD_CASTER(sot::VectorMultiBound, sotVMB);
-
- /* --- MULTI BOUND ---------------------------------------------------------- */
- /* --- MULTI BOUND ---------------------------------------------------------- */
- /* --- MULTI BOUND ---------------------------------------------------------- */
-
- DG_SIGNAL_CAST_DEFINITION_TRACE(sot::VectorMultiBound);
-
- void SignalCast<VectorMultiBound>::
- trace( const VectorMultiBound& t,std::ostream& os )
- {
- for( VectorMultiBound::const_iterator iter=t.begin();t.end()!=iter;++iter )
- {
- switch( iter->mode )
- {
- case MultiBound::MODE_SINGLE:
- os << iter->getSingleBound() << "\t";
- break;
- case MultiBound::MODE_DOUBLE:
- if( iter->getDoubleBoundSetup(MultiBound::BOUND_INF) )
- os << iter->getDoubleBound(MultiBound::BOUND_INF)<<"\t";
- else os <<"-inf\t";
- if( iter->getDoubleBoundSetup(MultiBound::BOUND_SUP) )
- os << iter->getDoubleBound(MultiBound::BOUND_SUP)<<"\t";
- else os <<"+inf\t";
- break;
- }
- }
- }
- DG_ADD_CASTER(sot::VectorMultiBound,sotVMB);
-
-
-}// namespace dynamicgraph
+} // namespace dynamicgraph
diff --git a/src/sot/flags.cpp b/src/sot/flags.cpp
index 2c4dc359..6df6414a 100644
--- a/src/sot/flags.cpp
+++ b/src/sot/flags.cpp
@@ -12,12 +12,12 @@
/* --------------------------------------------------------------------- */
/*! System framework */
-#include <stdlib.h>
#include <list>
+#include <stdlib.h>
/*! Local Framework */
-#include <sot/core/flags.hh>
#include <sot/core/debug.hh>
+#include <sot/core/flags.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -26,399 +26,434 @@ using namespace dynamicgraph::sot;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-static void displaybool( ostream& os, const char c, const bool reverse=false )
-{
- for( std::size_t j=sizeof(char)*8;j-->0; )
- {
- //os<<i<<","<<j<<": "<<c+0<<std::endl;
- if(reverse) os<< (!((c>>j)&0x1)); else os<< (((c>>j)&0x1));//?"1":"0");
- }
+static void displaybool(ostream &os, const char c, const bool reverse = false) {
+ for (std::size_t j = sizeof(char) * 8; j-- > 0;) {
+ // os<<i<<","<<j<<": "<<c+0<<std::endl;
+ if (reverse)
+ os << (!((c >> j) & 0x1));
+ else
+ os << (((c >> j) & 0x1)); //?"1":"0");
+ }
}
-static string displaybool(const char c, const bool reverse=false )
-{
+static string displaybool(const char c, const bool reverse = false) {
stringstream oss;
- for( std::size_t j=sizeof(char)*8;j-->0; )
- {
- //os<<i<<","<<j<<": "<<c+0<<std::endl;
- if(reverse) oss<< (!((c>>j)&0x1)); else oss<< (((c>>j)&0x1));//?"1":"0");
- }
+ for (std::size_t j = sizeof(char) * 8; j-- > 0;) {
+ // os<<i<<","<<j<<": "<<c+0<<std::endl;
+ if (reverse)
+ oss << (!((c >> j) & 0x1));
+ else
+ oss << (((c >> j) & 0x1)); //?"1":"0");
+ }
return oss.str();
}
/* --------------------------------------------------------------------- */
+Flags::Flags(const bool &b) : flags(), reverse(b) {}
-Flags::
-Flags( const bool& b ) : flags(),reverse(b) { }
-
-Flags::
-Flags( const char& c ) : flags(),reverse(false) { add(c); }
+Flags::Flags(const char &c) : flags(), reverse(false) { add(c); }
-Flags::
-Flags( const int& c4 ) : flags(),reverse(false) { add(c4);}
+Flags::Flags(const int &c4) : flags(), reverse(false) { add(c4); }
-Flags::
-operator bool ( void ) const
-{
- if(reverse) return true;
- for( unsigned int i=0;i<flags.size();++i ) if( flags[i] ) return true;
+Flags::operator bool(void) const {
+ if (reverse)
+ return true;
+ for (unsigned int i = 0; i < flags.size(); ++i)
+ if (flags[i])
+ return true;
return false;
}
/* --------------------------------------------------------------------- */
-char Flags::
-operator[] (const unsigned int& i) const
-{
+char Flags::operator[](const unsigned int &i) const {
char res;
- if( i<flags.size() )
+ if (i < flags.size())
res = flags[i];
- else res=0;
- //cout<<"["<<i<<"] "<<res+0<<"||"<<(!res)+0<<std::endl;
- if( reverse ) return static_cast<char>(~res);//(!res);
+ else
+ res = 0;
+ // cout<<"["<<i<<"] "<<res+0<<"||"<<(!res)+0<<std::endl;
+ if (reverse)
+ return static_cast<char>(~res); //(!res);
return res;
}
-namespace dynamicgraph { namespace sot {
-char operator>> (const Flags& f,const int& i)
-{
- const div_t q = div(i,8);
+namespace dynamicgraph {
+namespace sot {
+char operator>>(const Flags &f, const int &i) {
+ const div_t q = div(i, 8);
char res = static_cast<char>(f[q.quot] >> q.rem);
- res = static_cast<char>(res | f[q.quot+1] << (8-q.rem));
+ res = static_cast<char>(res | f[q.quot + 1] << (8 - q.rem));
return res;
}
-} /* namespace sot */} /* namespace dynamicgraph */
-
-bool Flags::
-operator() (const int& i) const
-{
- return ((*this)>>i)&0x01;
-}
+} /* namespace sot */
+} /* namespace dynamicgraph */
+bool Flags::operator()(const int &i) const { return ((*this) >> i) & 0x01; }
/* --------------------------------------------------------------------- */
-void Flags::
-add( const char& c )
-{ flags.push_back( c ); }
-
-void Flags::
-add( const int& c4 )
-{
- const char* c4p = (const char*)&c4;
- for(unsigned int i=0;i<sizeof(int);++i) add(c4p[i]);
+void Flags::add(const char &c) { flags.push_back(c); }
+
+void Flags::add(const int &c4) {
+ const char *c4p = (const char *)&c4;
+ for (unsigned int i = 0; i < sizeof(int); ++i)
+ add(c4p[i]);
}
/* --------------------------------------------------------------------- */
-void Flags::
-set( const unsigned int & idx )
-{
- unsigned int d= (idx/8), m=(idx%8);
-
- char brik = static_cast<char>((reverse)?(255-(1<<m)):(1<<m));
-
- if( flags.size()>d )
- {
- sotDEBUG(45) << "List long enough. Modify." << std::endl;
- char & el = flags[d];
- if( reverse ) el &= brik; else el |= brik;
+void Flags::set(const unsigned int &idx) {
+ unsigned int d = (idx / 8), m = (idx % 8);
+
+ char brik = static_cast<char>((reverse) ? (255 - (1 << m)) : (1 << m));
+
+ if (flags.size() > d) {
+ sotDEBUG(45) << "List long enough. Modify." << std::endl;
+ char &el = flags[d];
+ if (reverse)
+ el &= brik;
+ else
+ el |= brik;
+ } else {
+ sotDEBUG(45) << "List not long enough. Add " << flags.size() << " " << d
+ << std::endl;
+ if (!reverse) {
+ for (std::vector<char>::size_type i = flags.size(); i < d; ++i)
+ add((char)0);
+ add(brik);
}
- else
- {
- sotDEBUG(45) << "List not long enough. Add "
- << flags.size() <<" "<<d << std::endl;
- if(! reverse )
- {
- for( std::vector<char>::size_type i=flags.size();i<d;++i ) add((char)0);
- add(brik);
- }
- }
- sotDEBUG(45) << "New flag: "<< *this << endl;
+ }
+ sotDEBUG(45) << "New flag: " << *this << endl;
}
-void Flags::
-unset( const unsigned int & idx )
-{
- unsigned int d= (idx/8), m=(idx%8);
-
- char brik = static_cast<char>((reverse)?(1<<m):(255-(1<<m)));
- if( flags.size()>d )
- {
- sotDEBUG(45) << "List long enough. Modify." << std::endl;
- char & el = flags[d];
- if( reverse ) el |= brik; else el &= brik;
- }
- else
- {
- sotDEBUG(45) << "List not long enough. Add." << std::endl;
- if( reverse )
- {
- for( std::vector<char>::size_type i=flags.size();i<d;++i ) add((char)255);
- add(brik);
- }
+void Flags::unset(const unsigned int &idx) {
+ unsigned int d = (idx / 8), m = (idx % 8);
+
+ char brik = static_cast<char>((reverse) ? (1 << m) : (255 - (1 << m)));
+ if (flags.size() > d) {
+ sotDEBUG(45) << "List long enough. Modify." << std::endl;
+ char &el = flags[d];
+ if (reverse)
+ el |= brik;
+ else
+ el &= brik;
+ } else {
+ sotDEBUG(45) << "List not long enough. Add." << std::endl;
+ if (reverse) {
+ for (std::vector<char>::size_type i = flags.size(); i < d; ++i)
+ add((char)255);
+ add(brik);
}
- sotDEBUG(45) << "New flag: "<< *this << endl;
+ }
+ sotDEBUG(45) << "New flag: " << *this << endl;
}
-
-namespace dynamicgraph { namespace sot {
+namespace dynamicgraph {
+namespace sot {
/* --------------------------------------------------------------------- */
-Flags Flags::
-operator! (void) const
-{
+Flags Flags::operator!(void) const {
Flags res = *this;
- res.reverse=!reverse;
+ res.reverse = !reverse;
return res;
}
-Flags operator& ( const Flags& f1,const Flags& f2 )
-{
- Flags res = f1; res &= f2; return res;
+Flags operator&(const Flags &f1, const Flags &f2) {
+ Flags res = f1;
+ res &= f2;
+ return res;
}
-Flags operator| ( const Flags& f1,const Flags& f2 )
-{
- Flags res = f1; res |= f2; return res;
+Flags operator|(const Flags &f1, const Flags &f2) {
+ Flags res = f1;
+ res |= f2;
+ return res;
}
-Flags& Flags::
-operator&= ( const Flags& f2 )
-{
- Flags &f1=*this;
- const std::vector<char>::size_type max=std::max(flags.size(),f2.flags.size());
- if( flags.size()<max ){ flags.resize(max); }
- bool revres = reverse&&f2.reverse;
- char c; int pos = 0;
- for( unsigned int i=0;i<max;++i )
- {
- c=f1[i]&f2[i]; if(revres) c=static_cast<char>(0xff-c);
- flags[i]=c; if(c) pos=i+1;
- }
- flags.resize(pos);reverse=revres;
+Flags &Flags::operator&=(const Flags &f2) {
+ Flags &f1 = *this;
+ const std::vector<char>::size_type max =
+ std::max(flags.size(), f2.flags.size());
+ if (flags.size() < max) {
+ flags.resize(max);
+ }
+ bool revres = reverse && f2.reverse;
+ char c;
+ int pos = 0;
+ for (unsigned int i = 0; i < max; ++i) {
+ c = f1[i] & f2[i];
+ if (revres)
+ c = static_cast<char>(0xff - c);
+ flags[i] = c;
+ if (c)
+ pos = i + 1;
+ }
+ flags.resize(pos);
+ reverse = revres;
return *this;
}
-Flags& Flags::
-operator|= ( const Flags& f2 )
-{
- Flags &f1=*this;
- const std::vector<char>::size_type max=std::max(flags.size(),f2.flags.size());
- if( flags.size()<max ){ flags.resize(max); }
- bool revres = reverse||f2.reverse;
- char c; int pos = 0;
- for( unsigned int i=0;i<max;++i )
- {
- //cout<<"DGi ";displaybool(cout,f1[i],false); cout<<" "; displaybool(cout,f2[i],false); cout<<endl;
- c=f1[i]|f2[i];
- //cout<<"DGr ";displaybool(cout,c,false); cout<<" "; displaybool(cout,c,revres); cout<<endl;
- if(revres) c=static_cast<char>(0xff-c);
- flags[i]=c; if(c) pos=i+1;
- }
- flags.resize(pos); reverse=revres;
+Flags &Flags::operator|=(const Flags &f2) {
+ Flags &f1 = *this;
+ const std::vector<char>::size_type max =
+ std::max(flags.size(), f2.flags.size());
+ if (flags.size() < max) {
+ flags.resize(max);
+ }
+ bool revres = reverse || f2.reverse;
+ char c;
+ int pos = 0;
+ for (unsigned int i = 0; i < max; ++i) {
+ // cout<<"DGi ";displaybool(cout,f1[i],false); cout<<" ";
+ // displaybool(cout,f2[i],false); cout<<endl;
+ c = f1[i] | f2[i];
+ // cout<<"DGr ";displaybool(cout,c,false); cout<<" ";
+ // displaybool(cout,c,revres); cout<<endl;
+ if (revres)
+ c = static_cast<char>(0xff - c);
+ flags[i] = c;
+ if (c)
+ pos = i + 1;
+ }
+ flags.resize(pos);
+ reverse = revres;
return *this;
}
-Flags operator& ( const Flags& f1,const bool& b ){ if(b)return f1; else return Flags();}
-Flags operator| ( const Flags& f1,const bool& b ){ if(b)return Flags(true); else return f1;}
-Flags& Flags::
-operator&= ( const bool& b ){ if(!b) { flags.clear(); reverse=false; } return *this; }
-Flags& Flags::
-operator|= ( const bool& b ){ if(b) { flags.clear(); reverse=true; } return *this;}
-
+Flags operator&(const Flags &f1, const bool &b) {
+ if (b)
+ return f1;
+ else
+ return Flags();
+}
+Flags operator|(const Flags &f1, const bool &b) {
+ if (b)
+ return Flags(true);
+ else
+ return f1;
+}
+Flags &Flags::operator&=(const bool &b) {
+ if (!b) {
+ flags.clear();
+ reverse = false;
+ }
+ return *this;
+}
+Flags &Flags::operator|=(const bool &b) {
+ if (b) {
+ flags.clear();
+ reverse = true;
+ }
+ return *this;
+}
/* --------------------------------------------------------------------- */
-std::ostream& operator<< (std::ostream& os, const Flags& fl )
-{
- if( fl.reverse ) os << "...11111 ";
+std::ostream &operator<<(std::ostream &os, const Flags &fl) {
+ if (fl.reverse)
+ os << "...11111 ";
std::vector<char>::size_type s = fl.flags.size();
- for( unsigned int i=0;i<fl.flags.size();++i )
- {
- char c=fl.flags[s-i-1];
- displaybool(os,c,fl.reverse);
- os<<" " ;
- }
+ for (unsigned int i = 0; i < fl.flags.size(); ++i) {
+ char c = fl.flags[s - i - 1];
+ displaybool(os, c, fl.reverse);
+ os << " ";
+ }
return os;
}
-static unsigned char MASK [] = { 0,1,3,7,15,31,63,127,255 };
+static unsigned char MASK[] = {0, 1, 3, 7, 15, 31, 63, 127, 255};
-std::istream& operator>> (std::istream& is, Flags& fl )
-{
+std::istream &operator>>(std::istream &is, Flags &fl) {
sotDEBUGIN(15);
std::list<char> listing;
- unsigned char count = 0,total = 0;
- char c,cur=0;
- bool reverse=false,contin=true;
- do
- {
- is.get(c);
- sotDEBUG(25) << "Read " << total+0 <<endl;
-
- total++;
- if(! is.good() ) break;
- switch(c)
- {
- case '.':
- {
- if(total<=3)
- {
- if(total==3) { reverse=true; sotDEBUG(20) << " Reverse" << endl; }
- }
- else total=10;
- break;
- }
- case '0': cur = static_cast<char>(cur & ~(0x01 << (7-count++))) ; break;
- case '1': cur = static_cast<char>(cur | 0x01 << (7-count++)) ; break;
- case '#':
- {
- char cnot; is.get(cnot);
- if( cnot=='!' )
- fl = (! Flags::readIndexMatlab( is ));
- else
- { is.unget(); fl = Flags::readIndexMatlab( is ); }
-
- return is;
- }
- case '&':
- {
- char cnot; is.get(cnot);
- if( cnot=='!' )
- fl &= (! Flags::readIndexMatlab( is ));
- else
- { is.unget(); fl &= Flags::readIndexMatlab( is ); }
- return is;
- }
- case '|':
- {
- char cnot; is.get(cnot);
- if( cnot=='!' )
- fl |= (! Flags::readIndexMatlab( is ));
- else
- { is.unget(); fl |= (Flags::readIndexMatlab( is )); }
- return is;
- }
- default:
- is.unget();
- contin=false;
- }
- sotDEBUG(25) << "Get cur= " << displaybool(cur) <<endl;
-
- if( count==8 )
- {
- sotDEBUG(20) << "Store " << displaybool(cur) <<endl;
- count=0;
- listing.push_front(cur);
- cur=0;
- }
- }while( contin );
-
- sotDEBUG(20) << "finish with " << displaybool(cur) << " ("<<0+count<<")"<<endl;
- char insert = 0;
- fl.flags.resize(listing.size()+((count>0)?1:0));
- total=0;
- for( std::list<char>::iterator iter = listing.begin();iter!=listing.end();++iter )
- {
- insert = static_cast<char>((*iter)<<count);
- insert = static_cast<char>(insert & ~MASK[count]);
- cur = static_cast<char>((MASK[count])&(cur>>(8-count)));
-
- insert |= cur;
- cur = *iter;
-
- if( reverse ) fl.flags[total++] = static_cast<char>(~insert);
- else fl.flags[total++] = insert;
- sotDEBUG(25) << "Insert " << displaybool(insert) <<endl;
+ unsigned char count = 0, total = 0;
+ char c, cur = 0;
+ bool reverse = false, contin = true;
+ do {
+ is.get(c);
+ sotDEBUG(25) << "Read " << total + 0 << endl;
+
+ total++;
+ if (!is.good())
+ break;
+ switch (c) {
+ case '.': {
+ if (total <= 3) {
+ if (total == 3) {
+ reverse = true;
+ sotDEBUG(20) << " Reverse" << endl;
+ }
+ } else
+ total = 10;
+ break;
+ }
+ case '0':
+ cur = static_cast<char>(cur & ~(0x01 << (7 - count++)));
+ break;
+ case '1':
+ cur = static_cast<char>(cur | 0x01 << (7 - count++));
+ break;
+ case '#': {
+ char cnot;
+ is.get(cnot);
+ if (cnot == '!')
+ fl = (!Flags::readIndexMatlab(is));
+ else {
+ is.unget();
+ fl = Flags::readIndexMatlab(is);
+ }
+
+ return is;
+ }
+ case '&': {
+ char cnot;
+ is.get(cnot);
+ if (cnot == '!')
+ fl &= (!Flags::readIndexMatlab(is));
+ else {
+ is.unget();
+ fl &= Flags::readIndexMatlab(is);
+ }
+ return is;
+ }
+ case '|': {
+ char cnot;
+ is.get(cnot);
+ if (cnot == '!')
+ fl |= (!Flags::readIndexMatlab(is));
+ else {
+ is.unget();
+ fl |= (Flags::readIndexMatlab(is));
+ }
+ return is;
+ }
+ default:
+ is.unget();
+ contin = false;
}
+ sotDEBUG(25) << "Get cur= " << displaybool(cur) << endl;
- if( count>0 )
- {
- sotDEBUG(25) << "Cur fin " << displaybool(cur) <<endl;
- cur = static_cast<char>((MASK[count])&(cur>>(8-count)));
-
- sotDEBUG(25) << "Cur fin >> " <<8-count<<":" << displaybool(cur) <<endl;
- sotDEBUG(25) << "Mask fin "<<0+count<<": " << displaybool(MASK[count]) <<endl;
-
- cur = static_cast<char>(cur &MASK[count]);
- if( reverse )
- {
- cur = static_cast<char>(cur |~MASK[count]);
- fl.flags[total++] = static_cast<char>(~cur);
- }
- else
- {
- cur = static_cast<char>(cur &MASK[count]);
- fl.flags[total++] = cur;
- }
- sotDEBUG(25) << "Insert fin " << displaybool(cur) <<endl;
+ if (count == 8) {
+ sotDEBUG(20) << "Store " << displaybool(cur) << endl;
+ count = 0;
+ listing.push_front(cur);
+ cur = 0;
}
+ } while (contin);
- fl.reverse=reverse;
+ sotDEBUG(20) << "finish with " << displaybool(cur) << " (" << 0 + count << ")"
+ << endl;
+ char insert = 0;
+ fl.flags.resize(listing.size() + ((count > 0) ? 1 : 0));
+ total = 0;
+ for (std::list<char>::iterator iter = listing.begin(); iter != listing.end();
+ ++iter) {
+ insert = static_cast<char>((*iter) << count);
+ insert = static_cast<char>(insert & ~MASK[count]);
+ cur = static_cast<char>((MASK[count]) & (cur >> (8 - count)));
+
+ insert |= cur;
+ cur = *iter;
+
+ if (reverse)
+ fl.flags[total++] = static_cast<char>(~insert);
+ else
+ fl.flags[total++] = insert;
+ sotDEBUG(25) << "Insert " << displaybool(insert) << endl;
+ }
+
+ if (count > 0) {
+ sotDEBUG(25) << "Cur fin " << displaybool(cur) << endl;
+ cur = static_cast<char>((MASK[count]) & (cur >> (8 - count)));
+
+ sotDEBUG(25) << "Cur fin >> " << 8 - count << ":" << displaybool(cur)
+ << endl;
+ sotDEBUG(25) << "Mask fin " << 0 + count << ": " << displaybool(MASK[count])
+ << endl;
+
+ cur = static_cast<char>(cur & MASK[count]);
+ if (reverse) {
+ cur = static_cast<char>(cur | ~MASK[count]);
+ fl.flags[total++] = static_cast<char>(~cur);
+ } else {
+ cur = static_cast<char>(cur & MASK[count]);
+ fl.flags[total++] = cur;
+ }
+ sotDEBUG(25) << "Insert fin " << displaybool(cur) << endl;
+ }
+
+ fl.reverse = reverse;
sotDEBUGOUT(15);
return is;
}
-
/* --------------------------------------------------------------------- */
-const Flags FLAG_LINE_1( (char)0x1 );
-const Flags FLAG_LINE_2( (char)0x2 );
-const Flags FLAG_LINE_3( (char)0x4 );
-const Flags FLAG_LINE_4( (char)0x8 );
-const Flags FLAG_LINE_5( (char)0x10 );
-const Flags FLAG_LINE_6( (char)0x20 );
-const Flags FLAG_LINE_7( (char)0x40 );
-const Flags FLAG_LINE_8( (char)0x80 );
-
-} /* namespace sot */} /* namespace dynamicgraph */
+const Flags FLAG_LINE_1((char)0x1);
+const Flags FLAG_LINE_2((char)0x2);
+const Flags FLAG_LINE_3((char)0x4);
+const Flags FLAG_LINE_4((char)0x8);
+const Flags FLAG_LINE_5((char)0x10);
+const Flags FLAG_LINE_6((char)0x20);
+const Flags FLAG_LINE_7((char)0x40);
+const Flags FLAG_LINE_8((char)0x80);
+
+} /* namespace sot */
+} /* namespace dynamicgraph */
/* --------------------------------------------------------------------- */
-void Flags::
-readIndexMatlab( std::istream& cmdArgs,
- unsigned int & idx_beg,
- unsigned int &idx_end,
- bool& no_end )
-{
+void Flags::readIndexMatlab(std::istream &cmdArgs, unsigned int &idx_beg,
+ unsigned int &idx_end, bool &no_end) {
char col;
cmdArgs >> ws;
- if(! cmdArgs.good()) { idx_end=idx_beg=0; no_end=false; return; }
- cmdArgs.get(col); if( col==':' )
- { idx_beg=0; cmdArgs>>ws;}
- else
- {
- cmdArgs.putback(col); cmdArgs>>idx_beg>>ws;
- cmdArgs.get(col);
- if( col!=':' ) { idx_end=idx_beg; no_end=false; return; }
+ if (!cmdArgs.good()) {
+ idx_end = idx_beg = 0;
+ no_end = false;
+ return;
+ }
+ cmdArgs.get(col);
+ if (col == ':') {
+ idx_beg = 0;
+ cmdArgs >> ws;
+ } else {
+ cmdArgs.putback(col);
+ cmdArgs >> idx_beg >> ws;
+ cmdArgs.get(col);
+ if (col != ':') {
+ idx_end = idx_beg;
+ no_end = false;
+ return;
}
- cmdArgs>>ws;
- if( cmdArgs.good() )
- {
- sotDEBUG(15) << "Read end" << endl;
- cmdArgs >> idx_end; no_end=false;
- }
- else no_end = true;
-
- sotDEBUG(25) <<"Selec: " << idx_beg << " : "<< idx_end
- << "(" << no_end <<")"<<endl;
+ }
+ cmdArgs >> ws;
+ if (cmdArgs.good()) {
+ sotDEBUG(15) << "Read end" << endl;
+ cmdArgs >> idx_end;
+ no_end = false;
+ } else
+ no_end = true;
+
+ sotDEBUG(25) << "Selec: " << idx_beg << " : " << idx_end << "(" << no_end
+ << ")" << endl;
}
-Flags Flags::
-readIndexMatlab( std::istream& cmdArgs )
-{
- sotDEBUGIN(15) ;
- unsigned int idxEnd,idxStart;
+Flags Flags::readIndexMatlab(std::istream &cmdArgs) {
+ sotDEBUGIN(15);
+ unsigned int idxEnd, idxStart;
bool idxUnspec;
- readIndexMatlab( cmdArgs,idxStart,idxEnd,idxUnspec );
+ readIndexMatlab(cmdArgs, idxStart, idxEnd, idxUnspec);
- Flags newFlag( idxUnspec );
- if( idxUnspec )
- { for( unsigned int i=0;i<idxStart;++i ) newFlag.unset(i); }
- else { for( unsigned int i=idxStart;i<=idxEnd;++i ) newFlag.set(i); }
+ Flags newFlag(idxUnspec);
+ if (idxUnspec) {
+ for (unsigned int i = 0; i < idxStart; ++i)
+ newFlag.unset(i);
+ } else {
+ for (unsigned int i = idxStart; i <= idxEnd; ++i)
+ newFlag.set(i);
+ }
- sotDEBUG(25) << newFlag <<std::endl;
- sotDEBUGOUT(15) ;
+ sotDEBUG(25) << newFlag << std::endl;
+ sotDEBUGOUT(15);
return newFlag;
}
diff --git a/src/sot/memory-task-sot.cpp b/src/sot/memory-task-sot.cpp
index 78480d19..b0f1f1d8 100644
--- a/src/sot/memory-task-sot.cpp
+++ b/src/sot/memory-task-sot.cpp
@@ -7,67 +7,59 @@
*
*/
-#include <sot/core/memory-task-sot.hh>
#include <sot/core/debug.hh>
#include <sot/core/matrix-svd.hh>
+#include <sot/core/memory-task-sot.hh>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
const std::string MemoryTaskSOT::CLASS_NAME = "MemoryTaskSOT";
-
-MemoryTaskSOT::
-MemoryTaskSOT( const std::string & name
- ,const Matrix::Index nJ
- ,const Matrix::Index mJ
- ,const Matrix::Index ffsize )
- :
- Entity( name )
- ,jacobianInvSINOUT( "sotTaskAbstract("+name+")::inout(matrix)::Jinv" )
- ,jacobianConstrainedSINOUT( "sotTaskAbstract("+name+")::inout(matrix)::JK" )
- ,jacobianProjectedSINOUT( "sotTaskAbstract("+name+")::inout(matrix)::Jt" )
- ,singularBaseImageSINOUT( "sotTaskAbstract("+name+")::inout(matrix)::V" )
- ,rankSINOUT( "sotTaskAbstract("+name+")::inout(matrix)::rank" )
-{
- signalRegistration(jacobianInvSINOUT
- <<singularBaseImageSINOUT<<rankSINOUT
- <<jacobianConstrainedSINOUT<<jacobianProjectedSINOUT);
- initMemory( nJ,mJ,ffsize,true );
+MemoryTaskSOT::MemoryTaskSOT(const std::string &name, const Matrix::Index nJ,
+ const Matrix::Index mJ, const Matrix::Index ffsize)
+ : Entity(name),
+ jacobianInvSINOUT("sotTaskAbstract(" + name + ")::inout(matrix)::Jinv"),
+ jacobianConstrainedSINOUT("sotTaskAbstract(" + name +
+ ")::inout(matrix)::JK"),
+ jacobianProjectedSINOUT("sotTaskAbstract(" + name +
+ ")::inout(matrix)::Jt"),
+ singularBaseImageSINOUT("sotTaskAbstract(" + name +
+ ")::inout(matrix)::V"),
+ rankSINOUT("sotTaskAbstract(" + name + ")::inout(matrix)::rank") {
+ signalRegistration(jacobianInvSINOUT << singularBaseImageSINOUT << rankSINOUT
+ << jacobianConstrainedSINOUT
+ << jacobianProjectedSINOUT);
+ initMemory(nJ, mJ, ffsize, true);
}
+void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ,
+ const Matrix::Index ffsize,
+ bool atConstruction) {
+ sotDEBUG(15) << "Task-mermory " << getName() << ": resize " << nJ << "x" << mJ
+ << std::endl;
-void MemoryTaskSOT::
-initMemory( const Matrix::Index nJ,const Matrix::Index mJ,const Matrix::Index ffsize,
- bool atConstruction )
-{
- sotDEBUG(15) << "Task-mermory " << getName() << ": resize "
- << nJ << "x" << mJ << std::endl;
+ Jt.resize(nJ, mJ);
+ Jp.resize(mJ, nJ);
+ PJp.resize(mJ, nJ);
- Jt.resize( nJ,mJ );
- Jp.resize( mJ,nJ );
- PJp.resize( mJ,nJ );
+ Jff.resize(nJ, ffsize);
+ Jact.resize(nJ, mJ);
- Jff.resize( nJ,ffsize );
- Jact.resize( nJ,mJ );
+ JK.resize(nJ, mJ);
- JK.resize( nJ,mJ );
-
- svd = SVD_t (nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
-
- JK.fill(0);
- if (atConstruction) {
- Jt.setZero();
- Jp.setZero();
- PJp.setZero();
- Jff.setZero();
- Jact.setZero();
- JK.setZero();
- } else {
- Eigen::pseudoInverse(Jt,Jp);
- }
- }
+ svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV);
+ JK.fill(0);
+ if (atConstruction) {
+ Jt.setZero();
+ Jp.setZero();
+ PJp.setZero();
+ Jff.setZero();
+ Jact.setZero();
+ JK.setZero();
+ } else {
+ Eigen::pseudoInverse(Jt, Jp);
+ }
+}
-void MemoryTaskSOT::
-display( std::ostream& /*os*/ ) const {} //TODO
+void MemoryTaskSOT::display(std::ostream & /*os*/) const {} // TODO
diff --git a/src/sot/rotation-simple.cpp b/src/sot/rotation-simple.cpp
index b17786c0..8a08518c 100644
--- a/src/sot/rotation-simple.cpp
+++ b/src/sot/rotation-simple.cpp
@@ -9,12 +9,10 @@
#include <sot/core/rotation-simple.hh>
-
bool MATLAB::fullPrec = false;
-MATLAB::MATLAB( const RotationSimple& Qh,const unsigned int nJ)
-{
+MATLAB::MATLAB(const RotationSimple &Qh, const unsigned int nJ) {
- Matrix eye = Eigen::MatrixXd::Identity(nJ,nJ);
+ Matrix eye = Eigen::MatrixXd::Identity(nJ, nJ);
Qh.multiplyRight(eye);
initFromBubMatrix(eye);
}
diff --git a/src/sot/solver-hierarchical-inequalities.cpp b/src/sot/solver-hierarchical-inequalities.cpp
index 4a762ab2..46a68213 100644
--- a/src/sot/solver-hierarchical-inequalities.cpp
+++ b/src/sot/solver-hierarchical-inequalities.cpp
@@ -11,10 +11,8 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
//#define WITH_CHRONO
-
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <sot/core/debug.hh>
@@ -23,298 +21,309 @@
using namespace dynamicgraph::sot;
#ifndef WIN32
-# include <sys/time.h>
+#include <sys/time.h>
#else
-# include <sot/core/utils-windows.hh>
+#include <sot/core/utils-windows.hh>
#endif /*WIN32*/
-# include <boost/math/special_functions/fpclassify.hpp>
-#define FORTRAN_ID( id ) id##_
-
-
+#include <boost/math/special_functions/fpclassify.hpp>
+#define FORTRAN_ID(id) id##_
/* ---------------------------------------------------------- */
/* --- BINDING FORTRAN -------------------------------------- */
/* ---------------------------------------------------------- */
-#define LAPACK_DGEQP3 FORTRAN_ID( dgeqp3 )
-#define LAPACK_DGEQPF FORTRAN_ID( dgeqpf )
+#define LAPACK_DGEQP3 FORTRAN_ID(dgeqp3)
+#define LAPACK_DGEQPF FORTRAN_ID(dgeqpf)
extern "C" {
- void LAPACK_DGEQP3( const int* m, const int* n, double* a, const int* lda, int * jpvt,
- double* tau, double* work, const int* lwork, int* info );
- void LAPACK_DGEQPF( const int* m, const int* n, double* a, const int* lda, int * jpvt,
- double* tau, double* work, int* info );
+void LAPACK_DGEQP3(const int *m, const int *n, double *a, const int *lda,
+ int *jpvt, double *tau, double *work, const int *lwork,
+ int *info);
+void LAPACK_DGEQPF(const int *m, const int *n, double *a, const int *lda,
+ int *jpvt, double *tau, double *work, int *info);
}
-namespace boost { namespace numeric { namespace bindings { namespace lapack
- {
-
- template<typename bubTemplateMatrix>
- inline int geqp (bubTemplateMatrix &A,
- bub::vector< int >& jp, bubVector& tau)
- {
- int const mF= A.size1();
- int const nF= A.size2();
- if(( nF==0)||(mF==0)) return 0;
- ::boost::numeric::ublas::vector<double> work(std::max(1, nF*32));
-
- assert (nF <= (int)tau.size());
- assert (nF <= (int)work.size());
-
- ::boost::numeric::ublas::matrix<double> tmpA;
- tmpA = A;
- double* aF = MRAWDATA(tmpA);
-
- int const ldaF = traits::leading_dimension (A);
- int * jpvtF = VRAWDATA(jp);
- double * tauF = VRAWDATA(tau);
- double * workF = VRAWDATA(work);
- int const lworkF = work.size();
- int infoF;
-
- LAPACK_DGEQP3(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &lworkF, &infoF);
- //LAPACK_DGEQPF(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &infoF);
- return infoF;
- }
-
- }}}} // End Namespaces
-
-
+namespace boost {
+namespace numeric {
+namespace bindings {
+namespace lapack {
+
+template <typename bubTemplateMatrix>
+inline int geqp(bubTemplateMatrix &A, bub::vector<int> &jp, bubVector &tau) {
+ int const mF = A.size1();
+ int const nF = A.size2();
+ if ((nF == 0) || (mF == 0))
+ return 0;
+ ::boost::numeric::ublas::vector<double> work(std::max(1, nF * 32));
+
+ assert(nF <= (int)tau.size());
+ assert(nF <= (int)work.size());
+
+ ::boost::numeric::ublas::matrix<double> tmpA;
+ tmpA = A;
+ double *aF = MRAWDATA(tmpA);
+
+ int const ldaF = traits::leading_dimension(A);
+ int *jpvtF = VRAWDATA(jp);
+ double *tauF = VRAWDATA(tau);
+ double *workF = VRAWDATA(work);
+ int const lworkF = work.size();
+ int infoF;
+
+ LAPACK_DGEQP3(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &lworkF, &infoF);
+ // LAPACK_DGEQPF(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &infoF);
+ return infoF;
+}
-template< typename bubTemplateMatrix >
-void bubRemoveColumn( bubTemplateMatrix& M, const unsigned int col )
-{
- for( unsigned int j=col;j<M.size2()-1;++j )
- for( unsigned int i=0;i<M.size1();++i )
- M(i,j)=M(i,j+1);
- for( unsigned int i=0;i<M.size1();++i )
- M(i,M.size2()-1) = 0;
+} // namespace lapack
+} // namespace bindings
+} // namespace numeric
+} // namespace boost
+
+template <typename bubTemplateMatrix>
+void bubRemoveColumn(bubTemplateMatrix &M, const unsigned int col) {
+ for (unsigned int j = col; j < M.size2() - 1; ++j)
+ for (unsigned int i = 0; i < M.size1(); ++i)
+ M(i, j) = M(i, j + 1);
+ for (unsigned int i = 0; i < M.size1(); ++i)
+ M(i, M.size2() - 1) = 0;
}
-void bubRemoveColumn( bub::triangular_matrix<double,bub::upper>& M, const unsigned int col )
-{
- for( unsigned int j=col;j<M.size2()-1;++j )
- for( unsigned int i=0;i<=j;++i )
- M(i,j)=M(i,j+1);
- for( unsigned int i=0;i<M.size1();++i )
- M(i,M.size2()-1) = 0;
+void bubRemoveColumn(bub::triangular_matrix<double, bub::upper> &M,
+ const unsigned int col) {
+ for (unsigned int j = col; j < M.size2() - 1; ++j)
+ for (unsigned int i = 0; i <= j; ++i)
+ M(i, j) = M(i, j + 1);
+ for (unsigned int i = 0; i < M.size1(); ++i)
+ M(i, M.size2() - 1) = 0;
}
-template< typename bubTemplateMatrix >
-void bubRemoveColumn( bub::triangular_adaptor<bubTemplateMatrix,bub::upper> M,
- const unsigned int col )
-{
- for( unsigned int j=col;j<M.size2()-1;++j )
- for( unsigned int i=0;i<=j;++i )
- M(i,j)=M(i,j+1);
- for( unsigned int i=0;i<std::min(M.size1(),M.size2());++i )
- M(i,M.size2()-1) = 0;
+template <typename bubTemplateMatrix>
+void bubRemoveColumn(bub::triangular_adaptor<bubTemplateMatrix, bub::upper> M,
+ const unsigned int col) {
+ for (unsigned int j = col; j < M.size2() - 1; ++j)
+ for (unsigned int i = 0; i <= j; ++i)
+ M(i, j) = M(i, j + 1);
+ for (unsigned int i = 0; i < std::min(M.size1(), M.size2()); ++i)
+ M(i, M.size2() - 1) = 0;
}
-
-bub::indirect_array<>& operator+= ( bub::indirect_array<>& order,unsigned int _el )
-{
+bub::indirect_array<> &operator+=(bub::indirect_array<> &order,
+ unsigned int _el) {
const unsigned int N = order.size();
- bub::indirect_array<> o2(N+1);
- // std::copy(order.begin(),order.end(),o2.data().begin()); // Not working! Why?
- for( unsigned int i=0;i<N;++i ) o2[i]=order[i];
- o2[N]=_el;
- return order=o2;
+ bub::indirect_array<> o2(N + 1);
+ // std::copy(order.begin(),order.end(),o2.data().begin()); // Not working!
+ // Why?
+ for (unsigned int i = 0; i < N; ++i)
+ o2[i] = order[i];
+ o2[N] = _el;
+ return order = o2;
}
-bub::indirect_array<>& operator-= ( bub::indirect_array<>& order,unsigned int _el )
-{
+bub::indirect_array<> &operator-=(bub::indirect_array<> &order,
+ unsigned int _el) {
const unsigned int N = order.size();
- bub::indirect_array<> o2(N-1); unsigned int newi=0;
- for( unsigned int i=0;i<N;++i )
- {
- if(newi==N)
- {
- std::cerr << "Error while removing one elmt of <order>." << std::endl;
- throw "Error while removing one elmt of <order>.";
- }
- if(order[i]!=_el) o2[newi++]=order[i];
+ bub::indirect_array<> o2(N - 1);
+ unsigned int newi = 0;
+ for (unsigned int i = 0; i < N; ++i) {
+ if (newi == N) {
+ std::cerr << "Error while removing one elmt of <order>." << std::endl;
+ throw "Error while removing one elmt of <order>.";
}
- return order=o2;
+ if (order[i] != _el)
+ o2[newi++] = order[i];
+ }
+ return order = o2;
}
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-ConstraintMem::
-ConstraintMem( const ConstraintMem& clone )
- :active(clone.active),equality(clone.equality),notToBeConsidered(false)
- ,Ji(0),eiInf(clone.eiInf),eiSup(clone.eiSup)
- ,boundSide(clone.boundSide),activeSide(clone.activeSide)
- ,rankIncreaser(clone.rankIncreaser),constraintRow(clone.constraintRow)
- ,range(clone.range),lagrangian(clone.lagrangian)
- ,Ju(clone.Ju),Jdu(clone.Jdu)
-{
- if( clone.Ji.size() ) { Ji.resize(clone.Ji.size(),false); Ji.assign(clone.Ji); }
+ConstraintMem::ConstraintMem(const ConstraintMem &clone)
+ : active(clone.active), equality(clone.equality), notToBeConsidered(false),
+ Ji(0), eiInf(clone.eiInf), eiSup(clone.eiSup), boundSide(clone.boundSide),
+ activeSide(clone.activeSide), rankIncreaser(clone.rankIncreaser),
+ constraintRow(clone.constraintRow), range(clone.range),
+ lagrangian(clone.lagrangian), Ju(clone.Ju), Jdu(clone.Jdu) {
+ if (clone.Ji.size()) {
+ Ji.resize(clone.Ji.size(), false);
+ Ji.assign(clone.Ji);
+ }
sotDEBUG(15) << "ConstraintMem cloning" << std::endl;
}
-namespace dynamicgraph { namespace sot {
-std::ostream& operator<<( std::ostream& os,const ConstraintMem::BoundSideType& bs )
-{
- switch( bs )
- {
- case ConstraintMem::BOUND_VOID:
- os << "#";
- break;
- case ConstraintMem::BOUND_INF:
- os <<"-";
- break;
- case ConstraintMem::BOUND_SUP:
- os << "+";
- break;
- case ConstraintMem::BOUND_BOTH:
- os << "+/-";
- break;
- }
+namespace dynamicgraph {
+namespace sot {
+std::ostream &operator<<(std::ostream &os,
+ const ConstraintMem::BoundSideType &bs) {
+ switch (bs) {
+ case ConstraintMem::BOUND_VOID:
+ os << "#";
+ break;
+ case ConstraintMem::BOUND_INF:
+ os << "-";
+ break;
+ case ConstraintMem::BOUND_SUP:
+ os << "+";
+ break;
+ case ConstraintMem::BOUND_BOTH:
+ os << "+/-";
+ break;
+ }
return os;
}
-std::ostream & operator<< (std::ostream& os,const ConstraintMem &c )
-{
+std::ostream &operator<<(std::ostream &os, const ConstraintMem &c) {
os << "Cs[" << c.constraintRow << "] " << std::endl;
- if( c.Ji.size() ) os << "" << c.Ji;
- if( c.boundSide&ConstraintMem::BOUND_INF ) os << "/[-]" << c.eiInf;
- if( c.boundSide&ConstraintMem::BOUND_SUP ) os << "/[+]" << c.eiSup;
- if( c.active )
- { os << "\n\t-> active [" << c.activeSide
- << "] <rg=" << c.range << "> " << std::endl << "\t-> ";
- if( c.rankIncreaser ) os << "rank-inc " ;
- else { os << "non-rank-inc";}
- if(c.lagrangian!=0) os << std::endl << "\t-> l=" << c.lagrangian;
- if(c.equality )os << std::endl << "\t-> blocked(equality)";
- else os << std::endl << "\t-> floating(inequality)";
- }
- else
- {
- if(c.notToBeConsidered) os << "\n\t->not to be considered";
- else os << "\n\t-> inactive";
+ if (c.Ji.size())
+ os << "" << c.Ji;
+ if (c.boundSide & ConstraintMem::BOUND_INF)
+ os << "/[-]" << c.eiInf;
+ if (c.boundSide & ConstraintMem::BOUND_SUP)
+ os << "/[+]" << c.eiSup;
+ if (c.active) {
+ os << "\n\t-> active [" << c.activeSide << "] <rg=" << c.range << "> "
+ << std::endl
+ << "\t-> ";
+ if (c.rankIncreaser)
+ os << "rank-inc ";
+ else {
+ os << "non-rank-inc";
}
+ if (c.lagrangian != 0)
+ os << std::endl << "\t-> l=" << c.lagrangian;
+ if (c.equality)
+ os << std::endl << "\t-> blocked(equality)";
+ else
+ os << std::endl << "\t-> floating(inequality)";
+ } else {
+ if (c.notToBeConsidered)
+ os << "\n\t->not to be considered";
+ else
+ os << "\n\t-> inactive";
+ }
return os;
}
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
/* ---------------------------------------------------------- */
/* Specify the size of the constraint matrix, for pre-alocation. */
-void SolverHierarchicalInequalities::
-initConstraintSize( const unsigned int size )
-{
- if(Rh.size1()!=nJ) {Rh.resize(nJ,nJ,false); Rh.clear();}
- rankh=0;
- u0.resize(nJ,false); u0.clear();
- //constraintH.reserve(size);
- constraintS.reserve(size+1);
+void SolverHierarchicalInequalities::initConstraintSize(
+ const unsigned int size) {
+ if (Rh.size1() != nJ) {
+ Rh.resize(nJ, nJ, false);
+ Rh.clear();
+ }
+ rankh = 0;
+ u0.resize(nJ, false);
+ u0.clear();
+ // constraintH.reserve(size);
+ constraintS.reserve(size + 1);
}
-void SolverHierarchicalInequalities::
-setInitialCondition( const bubVector& _u0,
- const unsigned int _rankh )
-{
- u0.resize(nJ,false); u0.assign(_u0);
- rankh=_rankh; freeRank=nJ-rankh;
+void SolverHierarchicalInequalities::setInitialCondition(
+ const bubVector &_u0, const unsigned int _rankh) {
+ u0.resize(nJ, false);
+ u0.assign(_u0);
+ rankh = _rankh;
+ freeRank = nJ - rankh;
}
-void SolverHierarchicalInequalities::
-setInitialConditionVoid( void )
-{
- u0.resize(nJ,false); u0.clear();
- rankh=0; freeRank=nJ;
+void SolverHierarchicalInequalities::setInitialConditionVoid(void) {
+ u0.resize(nJ, false);
+ u0.clear();
+ rankh = 0;
+ freeRank = nJ;
}
-void SolverHierarchicalInequalities::
-setNbDof( const unsigned int _nJ )
-{
+void SolverHierarchicalInequalities::setNbDof(const unsigned int _nJ) {
sotDEBUGIN(15);
- if( nJ==_nJ) return;
- nJ=_nJ;
+ if (nJ == _nJ)
+ return;
+ nJ = _nJ;
Qh.resize(nJ);
- Rh.resize(nJ,nJ);
+ Rh.resize(nJ, nJ);
sotDEBUGOUT(15);
}
/* ---------------------------------------------------------- */
-void SolverHierarchicalInequalities::
-recordInitialConditions( void )
-{
- initialActiveH.resize(constraintH.size()); initialSideH.resize(constraintH.size());
+void SolverHierarchicalInequalities::recordInitialConditions(void) {
+ initialActiveH.resize(constraintH.size());
+ initialSideH.resize(constraintH.size());
std::vector<bool>::iterator iterBool = initialActiveH.begin();
ConstraintMem::BoundSideVector::iterator iterSide = initialSideH.begin();
ConstraintList::const_iterator iterCH = constraintH.begin();
- for( ;iterCH!=constraintH.end();++iterBool,++iterCH,++iterSide )
- {
- sotDEBUG(45) << "Initial " << iterCH->activeSide <<iterCH->active << std::endl;
- (*iterBool) = iterCH->active;
- (*iterSide) = iterCH->activeSide;
- sotDEBUG(45) << "Initial " << (*iterSide) << (*iterBool) << std::endl;
- }
+ for (; iterCH != constraintH.end(); ++iterBool, ++iterCH, ++iterSide) {
+ sotDEBUG(45) << "Initial " << iterCH->activeSide << iterCH->active
+ << std::endl;
+ (*iterBool) = iterCH->active;
+ (*iterSide) = iterCH->activeSide;
+ sotDEBUG(45) << "Initial " << (*iterSide) << (*iterBool) << std::endl;
+ }
- du0.resize(u0.size(),false); du0.assign(-u0);
+ du0.resize(u0.size(), false);
+ du0.assign(-u0);
}
-void SolverHierarchicalInequalities::
-computeDifferentialCondition( void )
-{
- if( constraintH.size()>initialActiveH.size() )
- {
- std::vector<bool>::const_iterator iterBool = initialActiveH.begin();
- ConstraintList::iterator iterCH = constraintH.begin();
- ConstraintMem::BoundSideVector::const_iterator iterSide = initialSideH.begin();
- toActivate.clear(); toInactivate.clear();
- for( ;iterBool!=initialActiveH.end();++iterBool,++iterCH,++iterSide )
- {
- ConstraintMem & ch = *iterCH;
- sotDEBUG(45) << "Initial " << (*iterSide)<<(*iterBool)
- << " - Final "<<ch.activeSide<< ch.active << std::endl;
- /* Constraint was active, and is not anymore or changed side. */
- if( (*iterBool)&&( (!ch.active)||(*iterSide!=ch.activeSide) ) )
- {
- toInactivate.push_back(ConstraintRef(ch.constraintRow));
- }
- /* Constraint is now active, but was inactive or changed side. */
- if( (ch.active)&&( (!(*iterBool))||(*iterSide!=ch.activeSide) ) )
- {
- toActivate.push_back(ConstraintRef(ch.constraintRow,ch.activeSide) );
- }
- }
+void SolverHierarchicalInequalities::computeDifferentialCondition(void) {
+ if (constraintH.size() > initialActiveH.size()) {
+ std::vector<bool>::const_iterator iterBool = initialActiveH.begin();
+ ConstraintList::iterator iterCH = constraintH.begin();
+ ConstraintMem::BoundSideVector::const_iterator iterSide =
+ initialSideH.begin();
+ toActivate.clear();
+ toInactivate.clear();
+ for (; iterBool != initialActiveH.end(); ++iterBool, ++iterCH, ++iterSide) {
+ ConstraintMem &ch = *iterCH;
+ sotDEBUG(45) << "Initial " << (*iterSide) << (*iterBool) << " - Final "
+ << ch.activeSide << ch.active << std::endl;
+ /* Constraint was active, and is not anymore or changed side. */
+ if ((*iterBool) && ((!ch.active) || (*iterSide != ch.activeSide))) {
+ toInactivate.push_back(ConstraintRef(ch.constraintRow));
+ }
+ /* Constraint is now active, but was inactive or changed side. */
+ if ((ch.active) && ((!(*iterBool)) || (*iterSide != ch.activeSide))) {
+ toActivate.push_back(ConstraintRef(ch.constraintRow, ch.activeSide));
+ }
}
- du0+=u0;
+ }
+ du0 += u0;
/* Compute the slack set to be initialize at first iteration. */
- unsigned int i=0; slackActiveSet.resize(constraintSactive.size());
- for( ConstraintRefList::const_iterator iter=constraintSactive.begin();
- iter!=constraintSactive.end();++iter,++i )
- {
- slackActiveSet[i].id = (*iter)->constraintRow;
- slackActiveSet[i].side = (*iter)->activeSide;
- }
+ unsigned int i = 0;
+ slackActiveSet.resize(constraintSactive.size());
+ for (ConstraintRefList::const_iterator iter = constraintSactive.begin();
+ iter != constraintSactive.end(); ++iter, ++i) {
+ slackActiveSet[i].id = (*iter)->constraintRow;
+ slackActiveSet[i].side = (*iter)->activeSide;
+ }
warmStartReady = true;
}
-void SolverHierarchicalInequalities::
-printDifferentialCondition( std::ostream & os ) const
-{
- if(! warmStartReady ) return;
+void SolverHierarchicalInequalities::printDifferentialCondition(
+ std::ostream &os) const {
+ if (!warmStartReady)
+ return;
os << "To activate = { ";
- for( std::vector<ConstraintRef>::const_iterator iterCH = toActivate.begin();
- toActivate.end()!=iterCH;++iterCH )
- { os << *iterCH << ", "; }
+ for (std::vector<ConstraintRef>::const_iterator iterCH = toActivate.begin();
+ toActivate.end() != iterCH; ++iterCH) {
+ os << *iterCH << ", ";
+ }
os << " }" << std::endl;
os << "To inactivate = { ";
- for( std::vector<ConstraintRef>::const_iterator iterCH = toInactivate.begin();
- toInactivate.end()!=iterCH;++iterCH )
- { os << *iterCH << ", "; }
+ for (std::vector<ConstraintRef>::const_iterator iterCH = toInactivate.begin();
+ toInactivate.end() != iterCH; ++iterCH) {
+ os << *iterCH << ", ";
+ }
os << " }" << std::endl;
- os <<"Active slack = { ";
- for( std::vector<ConstraintRef>::const_iterator iter=slackActiveSet.begin();
- iter!=slackActiveSet.end();++iter )
- { os << (*iter) << ", "; }
+ os << "Active slack = { ";
+ for (std::vector<ConstraintRef>::const_iterator iter = slackActiveSet.begin();
+ iter != slackActiveSet.end(); ++iter) {
+ os << (*iter) << ", ";
+ }
os << " }" << std::endl;
os << "du = " << (MATLAB)du0 << std::endl;
@@ -322,7 +331,6 @@ printDifferentialCondition( std::ostream & os ) const
/* ---------------------------------------------------------- */
-
// SolverHierarchicalInequalities::bubMatrixQROrdered
// SolverHierarchicalInequalities::
// accessQRs( void )
@@ -348,584 +356,590 @@ printDifferentialCondition( std::ostream & os ) const
// return QRord;
// }
SolverHierarchicalInequalities::bubMatrixQROrderedTriConst
-SolverHierarchicalInequalities::
-accessRsConst( void ) const
-{
- bubMatrixQRConst QRs( QhJsU,freeranges(),bub::range(0,sizes) );
+SolverHierarchicalInequalities::accessRsConst(void) const {
+ bubMatrixQRConst QRs(QhJsU, freeranges(), bub::range(0, sizes));
bubOrder iall(ranks);
- for( unsigned int i=0;i<ranks;++i ) iall(i)=i;
- bubMatrixQROrderedConst QRord( QRs,iall,orderS );
+ for (unsigned int i = 0; i < ranks; ++i)
+ iall(i) = i;
+ bubMatrixQROrderedConst QRord(QRs, iall, orderS);
return QRord;
}
-bub::triangular_adaptor<bub::matrix_range< const bubMatrix >,bub::upper>
-SolverHierarchicalInequalities::
-accessRhConst( void ) const
-{
- bub::matrix_range< const bubMatrix >
- Rhup( Rh,rangeh(),rangeh() );
+bub::triangular_adaptor<bub::matrix_range<const bubMatrix>, bub::upper>
+SolverHierarchicalInequalities::accessRhConst(void) const {
+ bub::matrix_range<const bubMatrix> Rhup(Rh, rangeh(), rangeh());
return Rhup;
}
-bub::triangular_adaptor<bub::matrix_range< bubMatrix >,bub::upper>
-SolverHierarchicalInequalities::
-accessRh( void )
-{
- bub::matrix_range< bubMatrix >
- Rhup( Rh,rangeh(),rangeh() );
+bub::triangular_adaptor<bub::matrix_range<bubMatrix>, bub::upper>
+SolverHierarchicalInequalities::accessRh(void) {
+ bub::matrix_range<bubMatrix> Rhup(Rh, rangeh(), rangeh());
return Rhup;
}
/* Assuming a diagonal-ordered triangular matrix. */
-template< typename bubTemplateMatrix >
-unsigned int SolverHierarchicalInequalities::
-rankDetermination( const bubTemplateMatrix& A,
- const double threshold )
-{
+template <typename bubTemplateMatrix>
+unsigned int
+SolverHierarchicalInequalities::rankDetermination(const bubTemplateMatrix &A,
+ const double threshold) {
unsigned int res = 0;
- for( unsigned int i=0;i<std::min(A.size1(),A.size2());++i )
- { if( fabs(A(i,i))>threshold ) res++; else break; }
+ for (unsigned int i = 0; i < std::min(A.size1(), A.size2()); ++i) {
+ if (fabs(A(i, i)) > threshold)
+ res++;
+ else
+ break;
+ }
return res;
}
/* ---------------------------------------------------------- */
#ifdef VP_DEBUG
-void SolverHierarchicalInequalities::
-displayConstraint( ConstraintList & cs )
-{
- for( ConstraintList::iterator iter=cs.begin();
- iter!=cs.end();++iter )
- {
- ConstraintMem & ci = *iter;
- sotDEBUG(25) << ci << std::endl;
- }
+void SolverHierarchicalInequalities::displayConstraint(ConstraintList &cs) {
+ for (ConstraintList::iterator iter = cs.begin(); iter != cs.end(); ++iter) {
+ ConstraintMem &ci = *iter;
+ sotDEBUG(25) << ci << std::endl;
+ }
}
#else
-void SolverHierarchicalInequalities::
-displayConstraint( ConstraintList & )
-{
-}
+void SolverHierarchicalInequalities::displayConstraint(ConstraintList &) {}
#endif //#ifdef VP_DEBUG
-
-
-void SolverHierarchicalInequalities::
-printDebug( void )
-{
+void SolverHierarchicalInequalities::printDebug(void) {
#ifdef VP_DEBUG
- sotDEBUG(15) << "constraintSactive:"<<std::endl;
- for( ConstraintRefList::const_iterator iter=constraintSactive.begin();
- iter!=constraintSactive.end();++iter )
- {
- ConstraintMem & cs = **iter;
- sotDEBUG(15) << "+"<<cs << std::endl;
- }
+ sotDEBUG(15) << "constraintSactive:" << std::endl;
+ for (ConstraintRefList::const_iterator iter = constraintSactive.begin();
+ iter != constraintSactive.end(); ++iter) {
+ ConstraintMem &cs = **iter;
+ sotDEBUG(15) << "+" << cs << std::endl;
+ }
- sotDEBUG(45) << "Sconstraints :: "<< std::endl;
+ sotDEBUG(45) << "Sconstraints :: " << std::endl;
displayConstraint(constraintS);
- sotDEBUG(25) << "Hconstraints :: "<< std::endl;
+ sotDEBUG(25) << "Hconstraints :: " << std::endl;
displayConstraint(constraintH);
- bubMatrix Jh( rankh,nJ ); /***/std::fill(Jh.data().begin(),Jh.data().end(),-1);
- for( ConstraintList::iterator iter=constraintH.begin();
- iter!=constraintH.end();++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.active && cs.rankIncreaser )
- {
- if(cs.notToBeConsidered) continue;
- if(cs.range<rankh) {bub::row(Jh,cs.range).assign( cs.Ji ); }
- else {sotDEBUG(1)<<"!!!" <<cs<<std::endl;}
- }
+ bubMatrix Jh(rankh, nJ); /***/
+ std::fill(Jh.data().begin(), Jh.data().end(), -1);
+ for (ConstraintList::iterator iter = constraintH.begin();
+ iter != constraintH.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active && cs.rankIncreaser) {
+ if (cs.notToBeConsidered)
+ continue;
+ if (cs.range < rankh) {
+ bub::row(Jh, cs.range).assign(cs.Ji);
+ } else {
+ sotDEBUG(1) << "!!!" << cs << std::endl;
+ }
}
+ }
sotDEBUG(15) << "rankh = " << rankh << std::endl;
sotDEBUG(15) << "Jh = " << (MATLAB)Jh << std::endl;
- sotDEBUG(15) << "Qh = " << MATLAB(Qh,nJ) << std::endl;
- sotDEBUG(15) << "QhJs = " << (MATLAB)bub::subrange(QhJs,0,nJ,0,sizes) << std::endl;
- sotDEBUG(15) << "QhJsU = " << (MATLAB)bub::subrange(QhJsU,0,nJ,0,sizes) << std::endl;
+ sotDEBUG(15) << "Qh = " << MATLAB(Qh, nJ) << std::endl;
+ sotDEBUG(15) << "QhJs = " << (MATLAB)bub::subrange(QhJs, 0, nJ, 0, sizes)
+ << std::endl;
+ sotDEBUG(15) << "QhJsU = " << (MATLAB)bub::subrange(QhJsU, 0, nJ, 0, sizes)
+ << std::endl;
sotDEBUG(15) << "u0 = " << MATLAB(u0) << std::endl;
{
- bubMatrix toto(nJ,nJ); toto = bub::prod(QhJs,bub::trans(QhJs));
- bubMatrix tata(nJ,nJ); tata = bub::prod(QhJsU,bub::trans(QhJsU));
- toto-=tata;
+ bubMatrix toto(nJ, nJ);
+ toto = bub::prod(QhJs, bub::trans(QhJs));
+ bubMatrix tata(nJ, nJ);
+ tata = bub::prod(QhJsU, bub::trans(QhJsU));
+ toto -= tata;
sotDEBUG(45) << "dQJJQ = " << (MATLAB)toto << std::endl;
}
bubMatrix Jht(bub::trans(Jh));
Qh.multiplyRightTranspose(Jht);
- bub::matrix_range<bubMatrix> Jht0(Jht,rangeh(),bub::range(0,rankh));
+ bub::matrix_range<bubMatrix> Jht0(Jht, rangeh(), bub::range(0, rankh));
sotDEBUG(55) << "QJh = " << (MATLAB)Jht0 << std::endl;
- Jht0-=bub::matrix_range<bubMatrix>(Rh,rangeh(),rangeh());
+ Jht0 -= bub::matrix_range<bubMatrix>(Rh, rangeh(), rangeh());
sotDEBUG(55) << "Rh = " << (MATLAB)Rh << std::endl;
sotDEBUG(55) << "Rh_QhJh = " << (MATLAB)Jht0 << std::endl;
sotDEBUG(15) << "||Rh_QhJh|| = " << bub::norm_1(Jht0) << std::endl;
- bubMatrix Js( sizes,nJ );
- for( ConstraintRefList::iterator iter=constraintSactive.begin();
- iter!=constraintSactive.end();++iter )
- {
- ConstraintMem & cs = **iter;
- if( cs.active )
- {
- if(cs.range<sizes) {bub::row(Js,cs.range).assign( cs.Ji ); }
- else {sotDEBUG(1)<<"!!!" <<cs<<std::endl;}
- }
+ bubMatrix Js(sizes, nJ);
+ for (ConstraintRefList::iterator iter = constraintSactive.begin();
+ iter != constraintSactive.end(); ++iter) {
+ ConstraintMem &cs = **iter;
+ if (cs.active) {
+ if (cs.range < sizes) {
+ bub::row(Js, cs.range).assign(cs.Ji);
+ } else {
+ sotDEBUG(1) << "!!!" << cs << std::endl;
+ }
}
+ }
sotDEBUG(45) << "Js = " << (MATLAB)Js << std::endl;
sotDEBUG(45) << "QhJs = " << (MATLAB)QhJs << std::endl;
bubMatrix Jst(bub::trans(Js));
Qh.multiplyRightTranspose(Jst);
- Jst-=bub::subrange(QhJs,0,nJ,0,sizes);
+ Jst -= bub::subrange(QhJs, 0, nJ, 0, sizes);
sotDEBUG(15) << "||Rs_QhJs|| = " << bub::norm_1(Jst) << std::endl;
-
-
-#endif //ifdef VP_DEBUG
+#endif // ifdef VP_DEBUG
}
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-void SolverHierarchicalInequalities::
-warmStart( void )
-{
- if(! warmStartReady ) return;
+void SolverHierarchicalInequalities::warmStart(void) {
+ if (!warmStartReady)
+ return;
ConstraintRefList toInactivateCH;
- for( std::vector<ConstraintRef>::const_iterator iter=toInactivate.begin();
- iter!=toInactivate.end();++iter )
- {
- toInactivateCH.push_back( &constraintH[iter->id] );
- }
- if( toInactivateCH.size()>0 ) forceDowndateHierachic(toInactivateCH);
+ for (std::vector<ConstraintRef>::const_iterator iter = toInactivate.begin();
+ iter != toInactivate.end(); ++iter) {
+ toInactivateCH.push_back(&constraintH[iter->id]);
+ }
+ if (toInactivateCH.size() > 0)
+ forceDowndateHierachic(toInactivateCH);
applyFreeSpaceMotion(du0);
ConstraintRefList toActivateCH;
ConstraintMem::BoundSideVector toActivateSide;
- for( std::vector<ConstraintRef>::const_iterator iter=toActivate.begin();
- iter!=toActivate.end();++iter )
- {
- toActivateCH.push_back( &constraintH[iter->id] );
- toActivateSide.push_back( iter->side );
- }
- if( toActivateCH.size()>0 )forceUpdateHierachic(toActivateCH,toActivateSide);
+ for (std::vector<ConstraintRef>::const_iterator iter = toActivate.begin();
+ iter != toActivate.end(); ++iter) {
+ toActivateCH.push_back(&constraintH[iter->id]);
+ toActivateSide.push_back(iter->side);
+ }
+ if (toActivateCH.size() > 0)
+ forceUpdateHierachic(toActivateCH, toActivateSide);
}
-void SolverHierarchicalInequalities::
-applyFreeSpaceMotion( const bubVector& _du )
-{
+void SolverHierarchicalInequalities::applyFreeSpaceMotion(
+ const bubVector &_du) {
printDebug();
du.assign(_du);
sotDEBUG(15) << "du = " << (MATLAB)du << std::endl;
/* Multiply by Ph. */
- Qh.multiplyRangeLeft(du,rankh,0);
+ Qh.multiplyRangeLeft(du, rankh, 0);
Qh.multiplyRight(du);
sotDEBUG(15) << "Phdu = " << (MATLAB)du << std::endl;
double tau;
selecActivationHierarchic(tau);
sotDEBUG(5) << "Warm start du limited by tau=" << tau << std::endl;
- du*=tau;
- u0+=du;
+ du *= tau;
+ u0 += du;
}
-void SolverHierarchicalInequalities::
-forceUpdateHierachic( ConstraintRefList& toUpdate,
- const ConstraintMem::BoundSideVector& boundSide )
-{
+void SolverHierarchicalInequalities::forceUpdateHierachic(
+ ConstraintRefList &toUpdate,
+ const ConstraintMem::BoundSideVector &boundSide) {
{
sotDEBUG(5) << "Now activating { ";
- ConstraintMem::BoundSideVector::const_iterator iterBound = boundSide.begin();
- for( ConstraintRefList::const_iterator iterCH = toUpdate.begin();
- toUpdate.end()!=iterCH;++iterCH,++iterBound )
- { sotDEBUGMUTE(5) << *iterBound << (*iterCH)->constraintRow << ", "; }
+ ConstraintMem::BoundSideVector::const_iterator iterBound =
+ boundSide.begin();
+ for (ConstraintRefList::const_iterator iterCH = toUpdate.begin();
+ toUpdate.end() != iterCH; ++iterCH, ++iterBound) {
+ sotDEBUGMUTE(5) << *iterBound << (*iterCH)->constraintRow << ", ";
+ }
sotDEBUGMUTE(5) << " }" << std::endl;
}
- sotDEBUG(15) << "/* Create the matrix Js by concatenation of matrix cs.Ji. */" << std::endl;
- unsigned int sizes=toUpdate.size();
- bubMatrix _Jse(sizes,nJ); bubVector _ese(sizes);
+ sotDEBUG(15) << "/* Create the matrix Js by concatenation of matrix cs.Ji. */"
+ << std::endl;
+ unsigned int sizes = toUpdate.size();
+ bubMatrix _Jse(sizes, nJ);
+ bubVector _ese(sizes);
- unsigned int col=0;
+ unsigned int col = 0;
ConstraintMem::BoundSideVector::const_iterator iterBound = boundSide.begin();
- for( ConstraintRefList::iterator iter=toUpdate.begin();
- toUpdate.end() != iter;++iter,++iterBound )
- {
- ConstraintMem & cs = **iter;
- if(! cs.active )
- {
- sotDEBUG(1) << "Activation WSH <" << *iterBound
- << cs.constraintRow << ">" << std::endl;
-
- sotDEBUG(45) <<cs<<std::endl;
- bub::row(_Jse,col).assign(cs.Ji);
- if( (*iterBound)==ConstraintMem::BOUND_INF )
- _ese(col)=cs.eiInf; else _ese(col)=cs.eiSup;
- col++;
- cs.active=false; cs.notToBeConsidered=true;
- cs.activeSide = *iterBound;
- }
+ for (ConstraintRefList::iterator iter = toUpdate.begin();
+ toUpdate.end() != iter; ++iter, ++iterBound) {
+ ConstraintMem &cs = **iter;
+ if (!cs.active) {
+ sotDEBUG(1) << "Activation WSH <" << *iterBound << cs.constraintRow << ">"
+ << std::endl;
+
+ sotDEBUG(45) << cs << std::endl;
+ bub::row(_Jse, col).assign(cs.Ji);
+ if ((*iterBound) == ConstraintMem::BOUND_INF)
+ _ese(col) = cs.eiInf;
else
- {
- sotDEBUG(1) << "Error: <" << cs.constraintRow << "> is already active. "
- << std::endl;
- }
+ _ese(col) = cs.eiSup;
+ col++;
+ cs.active = false;
+ cs.notToBeConsidered = true;
+ cs.activeSide = *iterBound;
+ } else {
+ sotDEBUG(1) << "Error: <" << cs.constraintRow << "> is already active. "
+ << std::endl;
}
- sotDEBUG(45) <<"Jwsh = " <<(MATLAB)_Jse<<std::endl;
- if(sizes>col) { sizes=col; _Jse.resize(sizes,nJ,true); _ese.resize(sizes,true); }
- sotDEBUG(25) <<"Jwsh = " <<(MATLAB)_Jse<<std::endl;
+ }
+ sotDEBUG(45) << "Jwsh = " << (MATLAB)_Jse << std::endl;
+ if (sizes > col) {
+ sizes = col;
+ _Jse.resize(sizes, nJ, true);
+ _ese.resize(sizes, true);
+ }
+ sotDEBUG(25) << "Jwsh = " << (MATLAB)_Jse << std::endl;
sotDEBUG(15) << "/* Solve these constraints. */" << std::endl;
- bubMatrix _Jsi(0,0); bubVector _esiInf(0),_esiSup(0);
+ bubMatrix _Jsi(0, 0);
+ bubVector _esiInf(0), _esiSup(0);
std::vector<ConstraintMem::BoundSideType> _esiBound(0);
- solve(_Jse,_ese,_Jsi,_esiInf,_esiSup,_esiBound,false);
+ solve(_Jse, _ese, _Jsi, _esiInf, _esiSup, _esiBound, false);
sotDEBUG(15) << "/* Copy S in H. */" << std::endl;
/* Pe is the range of the actual column of QR in the original
* matrix: QR[:,i] == Jse'[:,pe(i)]. */
sotDEBUG(15) << "orderS = " << (MATLAB)orderS << std::endl;
-// for( ConstraintList::iterator iter=constraintS.begin();
-// constraintS.end() != iter;++iter )
-// {
-// ConstraintMem & cs = *iter;
-// ConstraintMem & ch = *toUpdate[cs.constraintRow];
- unsigned int rangeInS=0;
- for( ConstraintRefList::iterator iter=toUpdate.begin();
- toUpdate.end() != iter;++iter )
- {
- ConstraintMem & ch = **iter;
- if(! ch.notToBeConsidered ) continue;
- ConstraintMem & cs = constraintS[rangeInS++];
- ch.range=cs.range+rankh;
- ch.rankIncreaser=cs.rankIncreaser;
- ch.equality=false; // TODO: ?? lock a constraint ??
- ch.notToBeConsidered = false; ch.active = true;
- sotDEBUG(15) << "Add eq FR: " << ch << std::endl;
- }
+ // for( ConstraintList::iterator iter=constraintS.begin();
+ // constraintS.end() != iter;++iter )
+ // {
+ // ConstraintMem & cs = *iter;
+ // ConstraintMem & ch = *toUpdate[cs.constraintRow];
+ unsigned int rangeInS = 0;
+ for (ConstraintRefList::iterator iter = toUpdate.begin();
+ toUpdate.end() != iter; ++iter) {
+ ConstraintMem &ch = **iter;
+ if (!ch.notToBeConsidered)
+ continue;
+ ConstraintMem &cs = constraintS[rangeInS++];
+ ch.range = cs.range + rankh;
+ ch.rankIncreaser = cs.rankIncreaser;
+ ch.equality = false; // TODO: ?? lock a constraint ??
+ ch.notToBeConsidered = false;
+ ch.active = true;
+ sotDEBUG(15) << "Add eq FR: " << ch << std::endl;
+ }
sotDEBUG(15) << "/* Copy a triangular of Rs in Rh. */" << std::endl;
sotRotationComposed Qlast;
- for( unsigned int i=0;i<ranks;++i )
- {
- typedef bub::matrix_column<bubMatrixQRWide> bubQJsCol;
- bubQJsCol QJk(QhJs,orderS(i)); Qlast.multiplyLeft(QJk);
- sotDEBUG(45) << "QhJs_" << i << " = " << (MATLAB)QJk << std::endl;
- bub::vector_range<bubQJsCol> Ftdown(QJk,freerange());
- double beta;
- double normSignFt // Norm with sign!
- = sotRotationSimpleHouseholder::householderExtraction(Ftdown,beta,THRESHOLD_ZERO);
- sotDEBUG(45) << "Ft_" << i << " = " << (MATLAB)Ftdown << std::endl;
- sotDEBUG(45) << "b_" << i << " = " << beta << std::endl;
- if(fabs(beta)>THRESHOLD_ZERO)
- { Qlast.pushBack(sotRotationSimpleHouseholder(Ftdown,beta)); }
+ for (unsigned int i = 0; i < ranks; ++i) {
+ typedef bub::matrix_column<bubMatrixQRWide> bubQJsCol;
+ bubQJsCol QJk(QhJs, orderS(i));
+ Qlast.multiplyLeft(QJk);
+ sotDEBUG(45) << "QhJs_" << i << " = " << (MATLAB)QJk << std::endl;
+ bub::vector_range<bubQJsCol> Ftdown(QJk, freerange());
+ double beta;
+ double normSignFt // Norm with sign!
+ = sotRotationSimpleHouseholder::householderExtraction(Ftdown, beta,
+ THRESHOLD_ZERO);
+ sotDEBUG(45) << "Ft_" << i << " = " << (MATLAB)Ftdown << std::endl;
+ sotDEBUG(45) << "b_" << i << " = " << beta << std::endl;
+ if (fabs(beta) > THRESHOLD_ZERO) {
+ Qlast.pushBack(sotRotationSimpleHouseholder(Ftdown, beta));
+ }
- bub::matrix_column<bubMatrix> Rhk(Rh,rankh);
- bub::project(Rhk,rangeh()).assign( bub::project(QJk,rangeh()) );
- bub::project(Rhk,freerange()).assign( bub::zero_vector<double>(freeRank) );
- Rh(rankh,rankh) = normSignFt;
- sotDEBUG(45) << "Rh_" << i << " = " << (MATLAB)Rhk << std::endl;
+ bub::matrix_column<bubMatrix> Rhk(Rh, rankh);
+ bub::project(Rhk, rangeh()).assign(bub::project(QJk, rangeh()));
+ bub::project(Rhk, freerange()).assign(bub::zero_vector<double>(freeRank));
+ Rh(rankh, rankh) = normSignFt;
+ sotDEBUG(45) << "Rh_" << i << " = " << (MATLAB)Rhk << std::endl;
- rankh++; freeRank--;
- }
+ rankh++;
+ freeRank--;
+ }
Qh.pushBack(Qlast);
}
-void SolverHierarchicalInequalities::
-forceDowndateHierachic( ConstraintRefList& toDowndate )
-{
+void SolverHierarchicalInequalities::forceDowndateHierachic(
+ ConstraintRefList &toDowndate) {
{
sotDEBUG(5) << "Now inactivating { ";
- for( ConstraintRefList::const_iterator iterCH = toDowndate.begin();
- toDowndate.end()!=iterCH;++iterCH )
- { sotDEBUGMUTE(5) << (*iterCH)->constraintRow << ", "; }
+ for (ConstraintRefList::const_iterator iterCH = toDowndate.begin();
+ toDowndate.end() != iterCH; ++iterCH) {
+ sotDEBUGMUTE(5) << (*iterCH)->constraintRow << ", ";
+ }
sotDEBUGMUTE(5) << " }" << std::endl;
}
unsigned int rankhDec = 0;
- sotDEBUG(15) << "/* Precompute the new structure of Rh matrix. */" << std::endl;
- std::vector<bool> colDeleted(rankh,false);
+ sotDEBUG(15) << "/* Precompute the new structure of Rh matrix. */"
+ << std::endl;
+ std::vector<bool> colDeleted(rankh, false);
std::vector<unsigned int> colOffset(rankh);
- for( ConstraintRefList::iterator iter=toDowndate.begin();
- toDowndate.end() != iter;++iter )
- {
- ConstraintMem & cs = **iter;
- if( cs.active && cs.rankIncreaser )
- {
- sotDEBUG(1) << "Inactivation WSH <" << cs.activeSide << cs.constraintRow << ">" << std::endl;
- sotDEBUG(45) << "Force downdate cs " << cs << std::endl;
- const unsigned int range = cs.range;
- colDeleted[range]=true;
- ++ rankhDec;
- cs.active = false;
- }
- else
- { sotDEBUG(1) << "Can not force down inactive constraint " << cs.constraintRow << std::endl; }
+ for (ConstraintRefList::iterator iter = toDowndate.begin();
+ toDowndate.end() != iter; ++iter) {
+ ConstraintMem &cs = **iter;
+ if (cs.active && cs.rankIncreaser) {
+ sotDEBUG(1) << "Inactivation WSH <" << cs.activeSide << cs.constraintRow
+ << ">" << std::endl;
+ sotDEBUG(45) << "Force downdate cs " << cs << std::endl;
+ const unsigned int range = cs.range;
+ colDeleted[range] = true;
+ ++rankhDec;
+ cs.active = false;
+ } else {
+ sotDEBUG(1) << "Can not force down inactive constraint "
+ << cs.constraintRow << std::endl;
}
+ }
{
unsigned int offset = 0;
- for( unsigned int i=0;i<rankh;++i )
- {if(colDeleted[i]) offset++; else colOffset[i]=offset;}
+ for (unsigned int i = 0; i < rankh; ++i) {
+ if (colDeleted[i])
+ offset++;
+ else
+ colOffset[i] = offset;
+ }
}
- for( ConstraintList::iterator iter=constraintH.begin();
- constraintH.end() != iter;++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.active )
- {
- const unsigned int range = cs.range;
- cs.range -= colOffset[range];
- }
+ for (ConstraintList::iterator iter = constraintH.begin();
+ constraintH.end() != iter; ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active) {
+ const unsigned int range = cs.range;
+ cs.range -= colOffset[range];
}
+ }
sotDEBUG(15) << "/* Reduce the Rh matrix. */" << std::endl;
- for( unsigned int j=0;j<rankh;++j )
- {
- const unsigned int offset = colOffset[j];
- if( (!colDeleted[j])&&(offset>0) )
- {
- sotDEBUG(15) << "Copy " << j << " in " << j-offset << std::endl;
- for( unsigned int i=0;i<rankh;++i )
- {
- Rh(i,j-offset)=Rh(i,j);
- }
- }
+ for (unsigned int j = 0; j < rankh; ++j) {
+ const unsigned int offset = colOffset[j];
+ if ((!colDeleted[j]) && (offset > 0)) {
+ sotDEBUG(15) << "Copy " << j << " in " << j - offset << std::endl;
+ for (unsigned int i = 0; i < rankh; ++i) {
+ Rh(i, j - offset) = Rh(i, j);
+ }
}
- rankh-=rankhDec;
- freeRank+=rankhDec;
+ }
+ rankh -= rankhDec;
+ freeRank += rankhDec;
sotDEBUG(15) << "/* Correct the pseudo-hessenberg matrix. */" << std::endl;
- bub::matrix_range< bubMatrix > Rhlim( Rh,bub::range(0,rankh+rankhDec),rangeh() );
+ bub::matrix_range<bubMatrix> Rhlim(Rh, bub::range(0, rankh + rankhDec),
+ rangeh());
sotDEBUG(15) << "Rh_hess = " << (MATLAB)Rhlim << std::endl;
- for( unsigned int j=0;j<rankh+rankhDec;++j )
- {
- const unsigned int offset = colOffset[j];
- if( (!colDeleted[j])&&(offset>0) )
- {
- for( unsigned int i=0;i<offset;++i )
- {
- sotRotationSimpleGiven Gr( Rh,j-i-1,j-i,j-offset ); Gr.inverse();
- Gr.multiplyRightTranspose(Rhlim);
- Qh.pushBack(Gr);
- sotDEBUG(55) << "Rh_down" << j << "x"<<i<<" = " << (MATLAB)Rh << std::endl;
- sotDEBUG(55) << "Gr" << j << "x"<<i<<" = " << Gr << std::endl;
- }
- }
+ for (unsigned int j = 0; j < rankh + rankhDec; ++j) {
+ const unsigned int offset = colOffset[j];
+ if ((!colDeleted[j]) && (offset > 0)) {
+ for (unsigned int i = 0; i < offset; ++i) {
+ sotRotationSimpleGiven Gr(Rh, j - i - 1, j - i, j - offset);
+ Gr.inverse();
+ Gr.multiplyRightTranspose(Rhlim);
+ Qh.pushBack(Gr);
+ sotDEBUG(55) << "Rh_down" << j << "x" << i << " = " << (MATLAB)Rh
+ << std::endl;
+ sotDEBUG(55) << "Gr" << j << "x" << i << " = " << Gr << std::endl;
+ }
}
+ }
sotDEBUG(5) << "Rh_down = " << (MATLAB)accessRh() << std::endl;
sotDEBUG(15) << "/* Activate rankd-def constraints. */" << std::endl;
- for( ConstraintList::iterator iter=constraintH.begin();
- constraintH.end() != iter;++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.active&&(!cs.rankIncreaser) )
- {
- bubVector QJk(cs.Ji); Qh.multiplyLeft(QJk);
- if(fabs(QJk(rankh))>THRESHOLD_ZERO)
- {
- sotDEBUG(5) << "No rank loss when force downdating => "
- << cs.constraintRow << std::endl;
- bub::vector_range<bubVector> Ftdown(QJk,freerange());
- double beta;
- double normSignFt // Norm with sign!
- = sotRotationSimpleHouseholder::householderExtraction(Ftdown,beta,THRESHOLD_ZERO);
- sotDEBUG(45) << "Ft_" << " = " << (MATLAB)Ftdown << std::endl;
- sotDEBUG(45) << "b_" << " = " << beta << std::endl;
- if(fabs(beta)>THRESHOLD_ZERO)
- { Qh.pushBack(sotRotationSimpleHouseholder(Ftdown,beta)); }
-
- bub::matrix_column<bubMatrix> Rhk(Rh,rankh);
- bub::project(Rhk,rangeh()).assign( bub::project(QJk,rangeh()) );
- bub::project(Rhk,freerange()).assign( bub::zero_vector<double>(freeRank) );
- Rh(rankh,rankh) = normSignFt;
- sotDEBUG(45) << "Rh" << " = " << (MATLAB)Rhk << std::endl;
-
- cs.range = rankh; cs.rankIncreaser =true;
- rankh++; freeRank--;
- }
+ for (ConstraintList::iterator iter = constraintH.begin();
+ constraintH.end() != iter; ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active && (!cs.rankIncreaser)) {
+ bubVector QJk(cs.Ji);
+ Qh.multiplyLeft(QJk);
+ if (fabs(QJk(rankh)) > THRESHOLD_ZERO) {
+ sotDEBUG(5) << "No rank loss when force downdating => "
+ << cs.constraintRow << std::endl;
+ bub::vector_range<bubVector> Ftdown(QJk, freerange());
+ double beta;
+ double normSignFt // Norm with sign!
+ = sotRotationSimpleHouseholder::householderExtraction(
+ Ftdown, beta, THRESHOLD_ZERO);
+ sotDEBUG(45) << "Ft_"
+ << " = " << (MATLAB)Ftdown << std::endl;
+ sotDEBUG(45) << "b_"
+ << " = " << beta << std::endl;
+ if (fabs(beta) > THRESHOLD_ZERO) {
+ Qh.pushBack(sotRotationSimpleHouseholder(Ftdown, beta));
}
+
+ bub::matrix_column<bubMatrix> Rhk(Rh, rankh);
+ bub::project(Rhk, rangeh()).assign(bub::project(QJk, rangeh()));
+ bub::project(Rhk, freerange())
+ .assign(bub::zero_vector<double>(freeRank));
+ Rh(rankh, rankh) = normSignFt;
+ sotDEBUG(45) << "Rh"
+ << " = " << (MATLAB)Rhk << std::endl;
+
+ cs.range = rankh;
+ cs.rankIncreaser = true;
+ rankh++;
+ freeRank--;
+ }
}
+ }
}
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
#ifdef WITH_CHRONO
-# define SOT_DEFINE_CHRONO \
- struct timeval t0,t1; \
+#define SOT_DEFINE_CHRONO \
+ struct timeval t0, t1; \
double dtsolver
-# define SOT_INIT_CHRONO \
- gettimeofday(&t0,NULL)
-# define SOT_CHRONO(txt) \
- gettimeofday(&t1,NULL); \
- dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ; \
- std::cout << "t_" << txt << " = " << dtsolver << " %ms" << std::endl; \
- gettimeofday(&t0,NULL)
+#define SOT_INIT_CHRONO gettimeofday(&t0, NULL)
+#define SOT_CHRONO(txt) \
+ gettimeofday(&t1, NULL); \
+ dtsolver = (t1.tv_sec - t0.tv_sec) * 1000. + \
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.; \
+ std::cout << "t_" << txt << " = " << dtsolver << " %ms" << std::endl; \
+ gettimeofday(&t0, NULL)
#else // ifdef WITH_CHRONO
-# define SOT_DEFINE_CHRONO
-# define SOT_INIT_CHRONO
-# define SOT_CHRONO(txt)
+#define SOT_DEFINE_CHRONO
+#define SOT_INIT_CHRONO
+#define SOT_CHRONO(txt)
#endif // ifdef WITH_CHRONO
/* ---------------------------------------------------------- */
SOT_DEFINE_CHRONO
-void SolverHierarchicalInequalities::
-solve( const bubMatrix& Jse, const bubVector& ese,
- const bubMatrix& Jsi, const bubVector& esiInf, const bubVector& esiSup,
- const std::vector<ConstraintMem::BoundSideType> esiBoundSide,
- bool pushBackAtTheEnd )
-{
+void SolverHierarchicalInequalities::solve(
+ const bubMatrix &Jse, const bubVector &ese, const bubMatrix &Jsi,
+ const bubVector &esiInf, const bubVector &esiSup,
+ const std::vector<ConstraintMem::BoundSideType> esiBoundSide,
+ bool pushBackAtTheEnd) {
std::vector<ConstraintRef> vectVoid;
- solve(Jse,ese,Jsi,esiInf,esiSup,esiBoundSide,vectVoid,pushBackAtTheEnd);
+ solve(Jse, ese, Jsi, esiInf, esiSup, esiBoundSide, vectVoid,
+ pushBackAtTheEnd);
}
-void SolverHierarchicalInequalities::
-solve( const bubMatrix& Jse, const bubVector& ese,
- const bubMatrix& Jsi, const bubVector& esiInf, const bubVector& esiSup,
- const ConstraintMem::BoundSideVector & esiBoundSide,
- const std::vector<ConstraintRef> & slackActiveWarmStart,
- bool pushBackAtTheEnd )
-{
+void SolverHierarchicalInequalities::solve(
+ const bubMatrix &Jse, const bubVector &ese, const bubMatrix &Jsi,
+ const bubVector &esiInf, const bubVector &esiSup,
+ const ConstraintMem::BoundSideVector &esiBoundSide,
+ const std::vector<ConstraintRef> &slackActiveWarmStart,
+ bool pushBackAtTheEnd) {
sotDEBUGIN(1);
- /*!*/SOT_INIT_CHRONO;
+ /*!*/ SOT_INIT_CHRONO;
- initializeConstraintMemory(Jse,ese,Jsi,esiInf,esiSup,esiBoundSide,slackActiveWarmStart);
- /*!*/SOT_CHRONO("copy");
+ initializeConstraintMemory(Jse, ese, Jsi, esiInf, esiSup, esiBoundSide,
+ slackActiveWarmStart);
+ /*!*/ SOT_CHRONO("copy");
- /***/sotDEBUG(15) << "/* Initialize [Qs Rs]. */" << std::endl;
+ /***/ sotDEBUG(15) << "/* Initialize [Qs Rs]. */" << std::endl;
initializeDecompositionSlack();
- /*!*/SOT_CHRONO("init");
-
- do
- {
-
-
-
- /***/sotDEBUG(15) << "* - LOOP 1 * - * - * - * - * - * - * - * - * - * - * - *" << std::endl;
- /***/sotDEBUG(15) << "/* Active/inactive H-constraint. */" << std::endl;
- if(Hactivation)
- {
- updateConstraintHierarchic( HactivationRef,HactivationSide );
- /*!*/SOT_CHRONO("UpdateH");
- Hactivation=false;
- /***/sotDEBUG(15) << "/* Update Rank-1 [Qs Rs]. */" << std::endl;
- updateRankOneDowndate();
- /*!*/SOT_CHRONO("rankoneDowndate");
+ /*!*/ SOT_CHRONO("init");
+
+ do {
+
+ /***/ sotDEBUG(15)
+ << "* - LOOP 1 * - * - * - * - * - * - * - * - * - * - * - *"
+ << std::endl;
+ /***/ sotDEBUG(15) << "/* Active/inactive H-constraint. */" << std::endl;
+ if (Hactivation) {
+ updateConstraintHierarchic(HactivationRef, HactivationSide);
+ /*!*/ SOT_CHRONO("UpdateH");
+ Hactivation = false;
+ /***/ sotDEBUG(15) << "/* Update Rank-1 [Qs Rs]. */" << std::endl;
+ updateRankOneDowndate();
+ /*!*/ SOT_CHRONO("rankoneDowndate");
+
+ } else if (Hinactivation) {
+ downdateConstraintHierarchic(HinactivationRef);
+ /*!*/ SOT_CHRONO("DowndateH");
+ Hinactivation = false;
+ /***/ sotDEBUG(15) << "/* Update Rank-1 [Qs Rs]. */" << std::endl;
+ updateRankOneUpdate();
+ /*!*/ SOT_CHRONO("rankoneUpdate");
+ }
- }
- else if( Hinactivation )
- {
- downdateConstraintHierarchic( HinactivationRef );
- /*!*/SOT_CHRONO("DowndateH");
- Hinactivation=false;
- /***/sotDEBUG(15) << "/* Update Rank-1 [Qs Rs]. */" << std::endl;
- updateRankOneUpdate();
- /*!*/SOT_CHRONO("rankoneUpdate");
- }
+ /***/ sotDEBUG(15) << "/* Activate/inactivate S-constraint. */"
+ << std::endl;
+ if (Sactivation) {
+ /***/ sotDEBUG(15) << "/* Activate S-constraint. */" << std::endl;
+ updateConstraintSlack(SactivationRef, SactivationSide);
+ /*!*/ SOT_CHRONO("UpS");
+ /***/ sotDEBUG(15) << "/* Activate S-constraint. */" << std::endl;
+ } else if (Sinactivation) {
+ /***/ sotDEBUG(15) << "/* Inactivate S-constraint. */" << std::endl;
+ downdateConstraintSlack(SinactivationRef);
+ /*!*/ SOT_CHRONO("DownS");
+ }
+ /***/ printDebug();
+ /*!*/ SOT_CHRONO("Print");
+
+ /***/ sotDEBUG(15) << "/* Compute Primal. */" << std::endl;
+ computePrimal();
+ /*!*/ SOT_CHRONO("primal");
+
+ Sactivation = false;
+ Sinactivation = false;
+ Hactivation = false;
+ Hinactivation = false;
+
+ /***/ sotDEBUG(15) << "/* Compute Slack. */" << std::endl;
+ u0 += du;
+ computeSlack();
+ u0 -= du;
+ /*!*/ SOT_CHRONO("Slack");
+ /***/ sotDEBUG(15) << "/* Selec S-activation. */" << std::endl;
+ Sactivation = selecActivationSlack();
+ /*!*/ SOT_CHRONO("SelecAcS");
+ if (Sactivation) {
+ continue;
+ }
- /***/sotDEBUG(15) << "/* Activate/inactivate S-constraint. */" << std::endl;
- if(Sactivation)
- {
- /***/sotDEBUG(15) << "/* Activate S-constraint. */" << std::endl;
- updateConstraintSlack( SactivationRef,SactivationSide );
- /*!*/SOT_CHRONO("UpS");
- /***/sotDEBUG(15) << "/* Activate S-constraint. */" << std::endl;
- }
- else if( Sinactivation )
- {
- /***/sotDEBUG(15) << "/* Inactivate S-constraint. */" << std::endl;
- downdateConstraintSlack( SinactivationRef );
- /*!*/SOT_CHRONO("DownS");
- }
- /***/printDebug();
- /*!*/SOT_CHRONO("Print");
-
- /***/sotDEBUG(15) << "/* Compute Primal. */" << std::endl;
- computePrimal();
- /*!*/SOT_CHRONO("primal");
-
- Sactivation=false;Sinactivation=false; Hactivation=false;Hinactivation=false;
-
- /***/sotDEBUG(15) << "/* Compute Slack. */" << std::endl;
- u0+=du; computeSlack(); u0-=du;
- /*!*/SOT_CHRONO("Slack");
- /***/sotDEBUG(15) << "/* Selec S-activation. */" << std::endl;
- Sactivation = selecActivationSlack();
- /*!*/SOT_CHRONO("SelecAcS");
- if(Sactivation) { continue; }
-
- /***/sotDEBUG(15) << "/* Selec H-activation. */" << std::endl;
- double tau;
- Hactivation = selecActivationHierarchic(tau);
- /*!*/SOT_CHRONO("SelecAcH");
- if( Hactivation )
- {
- /***/sotDEBUG(15) << "tau = " << tau << std::endl;
- du*=tau;
- u0+=du;
- /*!*/SOT_CHRONO("IncUtau");
- }
- else
- {
- u0+=du;
- /*!*/SOT_CHRONO("IncUsimple");
-
- // /***/sotDEBUG(15) << "/* Compute Slack. */" << std::endl;
- // computeSlack();
- // /*!*/SOT_CHRONO("Slack");
- // /***/sotDEBUG(15) << "/* Selec S-activation. */" << std::endl;
- // Sactivation = selecActivationSlack();
- // //if( Sactivation ) continue;
- // /*!*/SOT_CHRONO("SelecAcS");
- // /***/sotDEBUG(15) << "/* Selec S-inactivation. */" << std::endl;
- // Sinactivation = selecInactivationSlack();
- // //if( Sinactivation ) continue;
- // /*!*/SOT_CHRONO("SelecInacS");
-
- /***/sotDEBUG(15) << "/* Compute Lagrangian. */" << std::endl;
- computeLagrangian();
- /*!*/SOT_CHRONO("Lagrang");
- /***/sotDEBUG(15) << "/* Selec H-inactivation. */" << std::endl;
- Hinactivation = selecInactivationHierarchic();
- /*!*/SOT_CHRONO("SelecInacH");
-
- if(! Hinactivation )
- {
- /***/sotDEBUG(15) << "/* Selec S-inactivation. */" << std::endl;
- Sinactivation = selecInactivationSlack();
- /*!*/SOT_CHRONO("SelecInacS");
- if( Sinactivation ) { continue; }
- }
+ /***/ sotDEBUG(15) << "/* Selec H-activation. */" << std::endl;
+ double tau;
+ Hactivation = selecActivationHierarchic(tau);
+ /*!*/ SOT_CHRONO("SelecAcH");
+ if (Hactivation) {
+ /***/ sotDEBUG(15) << "tau = " << tau << std::endl;
+ du *= tau;
+ u0 += du;
+ /*!*/ SOT_CHRONO("IncUtau");
+ } else {
+ u0 += du;
+ /*!*/ SOT_CHRONO("IncUsimple");
+
+ // /***/sotDEBUG(15) << "/* Compute Slack. */" << std::endl;
+ // computeSlack();
+ // /*!*/SOT_CHRONO("Slack");
+ // /***/sotDEBUG(15) << "/* Selec S-activation. */" <<
+ // std::endl; Sactivation = selecActivationSlack();
+ // //if( Sactivation ) continue;
+ // /*!*/SOT_CHRONO("SelecAcS");
+ // /***/sotDEBUG(15) << "/* Selec S-inactivation. */" <<
+ // std::endl; Sinactivation = selecInactivationSlack();
+ // //if( Sinactivation ) continue;
+ // /*!*/SOT_CHRONO("SelecInacS");
+
+ /***/ sotDEBUG(15) << "/* Compute Lagrangian. */" << std::endl;
+ computeLagrangian();
+ /*!*/ SOT_CHRONO("Lagrang");
+ /***/ sotDEBUG(15) << "/* Selec H-inactivation. */" << std::endl;
+ Hinactivation = selecInactivationHierarchic();
+ /*!*/ SOT_CHRONO("SelecInacH");
+
+ if (!Hinactivation) {
+ /***/ sotDEBUG(15) << "/* Selec S-inactivation. */" << std::endl;
+ Sinactivation = selecInactivationSlack();
+ /*!*/ SOT_CHRONO("SelecInacS");
+ if (Sinactivation) {
+ continue;
}
- /***/sotDEBUG(3) << "u = " << (MATLAB)u0 << std::endl;
+ }
+ }
+ /***/ sotDEBUG(3) << "u = " << (MATLAB)u0 << std::endl;
- } while(Sactivation||Sinactivation||Hactivation||Hinactivation);
+ } while (Sactivation || Sinactivation || Hactivation || Hinactivation);
/* Push-back S-constraint. */
/* Push-back [ Qs Rs ]. */
- /*!*/SOT_CHRONO("void");
- if(pushBackAtTheEnd) pushBackSlackToHierarchy();
- /*!*/SOT_CHRONO("PBSlack");
+ /*!*/ SOT_CHRONO("void");
+ if (pushBackAtTheEnd)
+ pushBackSlackToHierarchy();
+ /*!*/ SOT_CHRONO("PBSlack");
printDebug();
- /***/sotDEBUGOUT(1);
+ /***/ sotDEBUGOUT(1);
}
/* ---------------------------------------------------------- */
-void SolverHierarchicalInequalities::
-initializeConstraintMemory( const bubMatrix& Jse, const bubVector& ese,
- const bubMatrix& Jsi, const bubVector& esiInf, const bubVector& esiSup,
- const ConstraintMem::BoundSideVector& esiBoundSide,
- const std::vector<ConstraintRef>& warmStartSide )
-{
+void SolverHierarchicalInequalities::initializeConstraintMemory(
+ const bubMatrix &Jse, const bubVector &ese, const bubMatrix &Jsi,
+ const bubVector &esiInf, const bubVector &esiSup,
+ const ConstraintMem::BoundSideVector &esiBoundSide,
+ const std::vector<ConstraintRef> &warmStartSide) {
// TODO ... activate those protection.
- // if(!( (esiInf.size()==esiSup.size())&&(esiInf.size()==esiBoundSide.size())&&(esiInf.size()==Jsi.size1())) )
+ // if(!(
+ // (esiInf.size()==esiSup.size())&&(esiInf.size()==esiBoundSide.size())&&(esiInf.size()==Jsi.size1()))
+ // )
// {
// sotERROR << "Error in the size of I matrices. " << std::endl;
// throw "Error in the size of I matrices. ";
@@ -938,469 +952,490 @@ initializeConstraintMemory( const bubMatrix& Jse, const bubVector& ese,
sotDEBUG(45) << "Je = " << (MATLAB)Jse << std::endl;
sotDEBUG(45) << "Ji = " << (MATLAB)Jsi << std::endl;
{
- sotDEBUG(25) <<"Warm start active slack = { ";
- for( std::vector<ConstraintRef>::const_iterator iter=warmStartSide.begin();
- iter!=warmStartSide.end();++iter )
- { sotDEBUGMUTE(25) << (*iter) << ", "; }
+ sotDEBUG(25) << "Warm start active slack = { ";
+ for (std::vector<ConstraintRef>::const_iterator iter =
+ warmStartSide.begin();
+ iter != warmStartSide.end(); ++iter) {
+ sotDEBUGMUTE(25) << (*iter) << ", ";
+ }
sotDEBUGMUTE(25) << " }" << std::endl;
}
- Hactivation=false; Hinactivation=false;
- constraintS.clear(); constraintS.resize(ese.size()+Jsi.size1());
+ Hactivation = false;
+ Hinactivation = false;
+ constraintS.clear();
+ constraintS.resize(ese.size() + Jsi.size1());
constraintSactive.resize(0);
- sotDEBUG(15) <<"/* Copy the Jacobian to memory. */" << std::endl;
+ sotDEBUG(15) << "/* Copy the Jacobian to memory. */" << std::endl;
unsigned int row = 0;
- const unsigned int sizee=ese.size();
- for( ConstraintList::iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter,++row )
- {
- ConstraintMem & cs = *iter;
-
- cs.Ji.resize(nJ,false);
- cs.constraintRow=row;
- cs.active=false;
- cs.range = 0; cs.rankIncreaser = false;
- if( row<sizee )
- {
- cs.Ji.assign(bub::row(Jse,row)); cs.eiInf=ese(row); cs.equality=true;
- cs.active = true;
- cs.boundSide = cs.activeSide = ConstraintMem::BOUND_INF;
- }
- else
- {
- const unsigned int rowi = row-sizee;
- cs.Ji.assign(bub::row(Jsi,rowi));
- cs.boundSide = ConstraintMem::BOUND_VOID;
- if( (esiBoundSide[rowi]&ConstraintMem::BOUND_INF) &&
- (!boost::math::isnan(esiInf(rowi))) )
- {
- cs.eiInf=esiInf(rowi);
- cs.boundSide = (ConstraintMem::BoundSideType)(cs.boundSide|ConstraintMem::BOUND_INF);
- }
- if( (esiBoundSide[rowi]&ConstraintMem::BOUND_SUP)&&
- (!boost::math::isnan(esiSup(rowi))) )
- {
- cs.eiSup=esiSup(rowi);
- cs.boundSide = (ConstraintMem::BoundSideType)(cs.boundSide|ConstraintMem::BOUND_SUP);
- }
- cs.equality=false;
- }
- sotDEBUG(15) << "Init " << cs << std::endl;
+ const unsigned int sizee = ese.size();
+ for (ConstraintList::iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter, ++row) {
+ ConstraintMem &cs = *iter;
+
+ cs.Ji.resize(nJ, false);
+ cs.constraintRow = row;
+ cs.active = false;
+ cs.range = 0;
+ cs.rankIncreaser = false;
+ if (row < sizee) {
+ cs.Ji.assign(bub::row(Jse, row));
+ cs.eiInf = ese(row);
+ cs.equality = true;
+ cs.active = true;
+ cs.boundSide = cs.activeSide = ConstraintMem::BOUND_INF;
+ } else {
+ const unsigned int rowi = row - sizee;
+ cs.Ji.assign(bub::row(Jsi, rowi));
+ cs.boundSide = ConstraintMem::BOUND_VOID;
+ if ((esiBoundSide[rowi] & ConstraintMem::BOUND_INF) &&
+ (!boost::math::isnan(esiInf(rowi)))) {
+ cs.eiInf = esiInf(rowi);
+ cs.boundSide = (ConstraintMem::BoundSideType)(cs.boundSide |
+ ConstraintMem::BOUND_INF);
+ }
+ if ((esiBoundSide[rowi] & ConstraintMem::BOUND_SUP) &&
+ (!boost::math::isnan(esiSup(rowi)))) {
+ cs.eiSup = esiSup(rowi);
+ cs.boundSide = (ConstraintMem::BoundSideType)(cs.boundSide |
+ ConstraintMem::BOUND_SUP);
+ }
+ cs.equality = false;
}
+ sotDEBUG(15) << "Init " << cs << std::endl;
+ }
/* Activation Warm start. */
{
- for( std::vector<ConstraintRef>::const_iterator iter=warmStartSide.begin();
- iter!=warmStartSide.end();++iter )
- {
- if( iter->id<constraintS.size() )
- {
- constraintS[iter->id].active=true;
- constraintS[iter->id].activeSide=iter->side;
- sotDEBUG(1) << "Activation WS < " << *iter << ">" << std::endl;
- }
- }
+ for (std::vector<ConstraintRef>::const_iterator iter =
+ warmStartSide.begin();
+ iter != warmStartSide.end(); ++iter) {
+ if (iter->id < constraintS.size()) {
+ constraintS[iter->id].active = true;
+ constraintS[iter->id].activeSide = iter->side;
+ sotDEBUG(1) << "Activation WS < " << *iter << ">" << std::endl;
+ }
}
+ }
}
-void SolverHierarchicalInequalities::
-initializeDecompositionSlack(void)
-{
- QhJsU.resize(nJ,constraintS.size(),false); QhJsU.clear();
- QhJs.resize(nJ,constraintS.size(),false); QhJs.clear();
- Sactivation=false; Sinactivation=false;
-
- sotDEBUG(15) <<"/* 1.a Compute the limited matrix. */" << std::endl;
- sizes=0;
- for( ConstraintList::iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter )
- {
- ConstraintMem & cs = *iter;
- if(cs.active)
- { bub::column(QhJsU,sizes++) = cs.Ji; }
+void SolverHierarchicalInequalities::initializeDecompositionSlack(void) {
+ QhJsU.resize(nJ, constraintS.size(), false);
+ QhJsU.clear();
+ QhJs.resize(nJ, constraintS.size(), false);
+ QhJs.clear();
+ Sactivation = false;
+ Sinactivation = false;
+
+ sotDEBUG(15) << "/* 1.a Compute the limited matrix. */" << std::endl;
+ sizes = 0;
+ for (ConstraintList::iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active) {
+ bub::column(QhJsU, sizes++) = cs.Ji;
}
- bubMatrixQR QhJeU(QhJsU,fullrange(),bub::range(0,sizes));
- /*!*/SOT_CHRONO("BuildJs");
+ }
+ bubMatrixQR QhJeU(QhJsU, fullrange(), bub::range(0, sizes));
+ /*!*/ SOT_CHRONO("BuildJs");
Qh.multiplyRightTranspose(QhJeU);
sotDEBUG(15) << "QhJs = " << (MATLAB)QhJeU << std::endl;
- /*!*/SOT_CHRONO("QhJs");
-
- if( freeRank==0 )
- {
- ranks=0; orderS = bubOrder(0);
- bub::project(QhJs,fullrange(),bub::range(0,sizes)) .assign(QhJeU);
- /* Initialize the constraintMem. */
- unsigned int row=0;
- for( ConstraintList::iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter,++row )
- {
- ConstraintMem & cs = *iter; cs.range=row;
- if( cs.active ){ constraintSactive.push_back(&cs); }
- }
- sotDEBUG(15)<<"Freerank null, initial decomposition void."<<std::endl;
- return;
+ /*!*/ SOT_CHRONO("QhJs");
+
+ if (freeRank == 0) {
+ ranks = 0;
+ orderS = bubOrder(0);
+ bub::project(QhJs, fullrange(), bub::range(0, sizes)).assign(QhJeU);
+ /* Initialize the constraintMem. */
+ unsigned int row = 0;
+ for (ConstraintList::iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter, ++row) {
+ ConstraintMem &cs = *iter;
+ cs.range = row;
+ if (cs.active) {
+ constraintSactive.push_back(&cs);
+ }
}
+ sotDEBUG(15) << "Freerank null, initial decomposition void." << std::endl;
+ return;
+ }
- sotDEBUG(15) <<"/* 1.b Compute the QR decomposition. */" << std::endl;
- bubMatrixQR QRs(QhJsU,bub::range(rankh,nJ),bub::range(0,sizes));
- bubVector betas(sizes); betas.clear();
- bub::vector<int> orderSe(sizes); orderSe.clear();
- boost::numeric::bindings::lapack::geqp(QRs,orderSe,betas);
+ sotDEBUG(15) << "/* 1.b Compute the QR decomposition. */" << std::endl;
+ bubMatrixQR QRs(QhJsU, bub::range(rankh, nJ), bub::range(0, sizes));
+ bubVector betas(sizes);
+ betas.clear();
+ bub::vector<int> orderSe(sizes);
+ orderSe.clear();
+ boost::numeric::bindings::lapack::geqp(QRs, orderSe, betas);
ranks = rankDetermination(QRs);
sotDEBUG(15) << "ranks = " << ranks << std::endl;
sotDEBUG(15) << "QRs = " << (MATLAB)QhJeU << std::endl;
sotDEBUG(15) << "orderSe = " << orderSe << std::endl;
- /*!*/SOT_CHRONO("QRdecomp");
+ /*!*/ SOT_CHRONO("QRdecomp");
- sotDEBUG(15) <<"/* 1.c Organize into Q2 and R2. */" << std::endl;
+ sotDEBUG(15) << "/* 1.c Organize into Q2 and R2. */" << std::endl;
/* If Je is rank deficient, then the last <m-r> householder vectors
* do not affect the range basis M (where Q=[M N]). If M is constant
* wrt these vector, then Span(N)=orth(Span(M)) is also constant, even
* if N is non constant, and thus these vectors can be neglect. */
- sotRotationComposed Qs; Qs.householderQRinit( QRs,betas,ranks );
+ sotRotationComposed Qs;
+ Qs.householderQRinit(QRs, betas, ranks);
Qh.pushBack(Qs);
/* Remove the lower triangle (householder vectors). */
- for( unsigned int j=0;j<sizes;++j )
- for( unsigned int i=j+1;i<freeRank;++i ) QRs(i,j)=0;
- sotDEBUG(45) << "Qs = " << MATLAB(Qs,nJ) << std::endl;
+ for (unsigned int j = 0; j < sizes; ++j)
+ for (unsigned int i = j + 1; i < freeRank; ++i)
+ QRs(i, j) = 0;
+ sotDEBUG(45) << "Qs = " << MATLAB(Qs, nJ) << std::endl;
sotDEBUG(15) << "QJs = " << (MATLAB)QhJeU << std::endl;
- /*!*/SOT_CHRONO("QRorganize");
+ /*!*/ SOT_CHRONO("QRorganize");
/* Save QhJs (without U). */
- for( unsigned int j=0;j<sizes;++j )
- {
- for( unsigned int i=0;i<rankh;++i ) { QhJs(i,j) = QhJeU(i,orderSe(j)-1); }
- for( unsigned int i=rankh;i<nJ;++i ){ QhJs(i,j) = QhJeU(i,j); }
+ for (unsigned int j = 0; j < sizes; ++j) {
+ for (unsigned int i = 0; i < rankh; ++i) {
+ QhJs(i, j) = QhJeU(i, orderSe(j) - 1);
}
- QhJeU = bub::project(QhJs,fullrange(),bub::range(0,sizes));
- orderS = bubOrder(ranks); for( unsigned int i=0;i<ranks;++i ) orderS[i]=i;
+ for (unsigned int i = rankh; i < nJ; ++i) {
+ QhJs(i, j) = QhJeU(i, j);
+ }
+ }
+ QhJeU = bub::project(QhJs, fullrange(), bub::range(0, sizes));
+ orderS = bubOrder(ranks);
+ for (unsigned int i = 0; i < ranks; ++i)
+ orderS[i] = i;
sotDEBUG(15) << "QhsJs = " << (MATLAB)QhJeU << std::endl;
sotDEBUG(15) << "orderS = " << (MATLAB)orderS << std::endl;
- /*!*/SOT_CHRONO("SaveQhJs");
+ /*!*/ SOT_CHRONO("SaveQhJs");
/* Full range decomposition R0 = Qh.Js.U . */
regularizeQhJsU();
- /*!*/SOT_CHRONO("FullRankDecompo");
+ /*!*/ SOT_CHRONO("FullRankDecompo");
/* Initialize the constraintMem. */
ConstraintRefList activeOrderRaw;
- for( ConstraintList::iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.active ) activeOrderRaw.push_back(&cs);
- }
- /* Push the constraint in cSactive in the order decided by the QR decomposition. */
- for( unsigned int i=0;i<sizes;++i )
- {
- ConstraintMem & cs = *activeOrderRaw[orderSe[i]-1];
- if(i<ranks){ cs.rankIncreaser = true; }
- cs.range = i;
- constraintSactive.push_back(&cs);
- sotDEBUG(15) << "Activate " << cs << std::endl;
+ for (ConstraintList::iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active)
+ activeOrderRaw.push_back(&cs);
+ }
+ /* Push the constraint in cSactive in the order decided by the QR
+ * decomposition. */
+ for (unsigned int i = 0; i < sizes; ++i) {
+ ConstraintMem &cs = *activeOrderRaw[orderSe[i] - 1];
+ if (i < ranks) {
+ cs.rankIncreaser = true;
}
- /*!*/SOT_CHRONO("InitActiveMem");
+ cs.range = i;
+ constraintSactive.push_back(&cs);
+ sotDEBUG(15) << "Activate " << cs << std::endl;
+ }
+ /*!*/ SOT_CHRONO("InitActiveMem");
}
-
-
-
/* ---------------------------------------------------------- */
/* <constraintId> is the number of the constraint in the constraintH list. */
-void SolverHierarchicalInequalities::
-updateConstraintHierarchic( const unsigned int constraintId,
- const ConstraintMem::BoundSideType side )
-{
+void SolverHierarchicalInequalities::updateConstraintHierarchic(
+ const unsigned int constraintId, const ConstraintMem::BoundSideType side) {
sotDEBUG(15) << "khup = " << constraintId << std::endl;
- ConstraintMem & chup = constraintH[constraintId];
+ ConstraintMem &chup = constraintH[constraintId];
/* Compute the limited Jacobian. */
- bub::matrix_column<bubMatrix> QtJt(Rh,rankh);
+ bub::matrix_column<bubMatrix> QtJt(Rh, rankh);
QtJt.assign(chup.Ji);
Qh.multiplyLeft(QtJt);
sotDEBUG(15) << "Jk = " << (MATLAB)chup.Ji << std::endl;
sotDEBUG(15) << "QJk = " << (MATLAB)QtJt << std::endl;
- chup.rankIncreaser=false;
- bub::triangular_adaptor<bub::matrix_range< bubMatrix >,bub::upper>
- Rhprev = accessRh();
- if(rankh<nJ)
- {
- lastRotation.clear();
- /* Given rotation to nullify the new column. */
- for( unsigned int row=nJ-1;row>rankh;row-- )
- {
- if( fabs(QtJt(row))>THRESHOLD_ZERO )
- {
- chup.rankIncreaser=true;
- sotRotationSimpleGiven gr(Rh,row-1,row,rankh);
- gr.inverse();
- gr.multiplyLeft(QtJt);
- lastRotation.pushBack(gr);
- }
- }
- sotDEBUG(45) << "Add GR = " << lastRotation << std::endl;
- Qh.pushBack(lastRotation);
-
- /* Update information about the constraint. */
- chup.active = true; chup.rankIncreaser=true; chup.activeSide = side;
- chup.range=rankh;
- /* Increase the number of data. */
- rankh++; freeRank--; freeRankChange = -1;
+ chup.rankIncreaser = false;
+ bub::triangular_adaptor<bub::matrix_range<bubMatrix>, bub::upper> Rhprev =
+ accessRh();
+ if (rankh < nJ) {
+ lastRotation.clear();
+ /* Given rotation to nullify the new column. */
+ for (unsigned int row = nJ - 1; row > rankh; row--) {
+ if (fabs(QtJt(row)) > THRESHOLD_ZERO) {
+ chup.rankIncreaser = true;
+ sotRotationSimpleGiven gr(Rh, row - 1, row, rankh);
+ gr.inverse();
+ gr.multiplyLeft(QtJt);
+ lastRotation.pushBack(gr);
+ }
}
+ sotDEBUG(45) << "Add GR = " << lastRotation << std::endl;
+ Qh.pushBack(lastRotation);
+
+ /* Update information about the constraint. */
+ chup.active = true;
+ chup.rankIncreaser = true;
+ chup.activeSide = side;
+ chup.range = rankh;
+ /* Increase the number of data. */
+ rankh++;
+ freeRank--;
+ freeRankChange = -1;
+ }
- if(!chup.rankIncreaser)
- {
- std::cerr << "Error, H-up should always be full rank!" << std::endl;
- throw "Error, H-up should always be full rank!";
- }
+ if (!chup.rankIncreaser) {
+ std::cerr << "Error, H-up should always be full rank!" << std::endl;
+ throw "Error, H-up should always be full rank!";
+ }
sotDEBUG(15) << "Rh = " << (MATLAB)accessRhConst() << std::endl;
sotDEBUG(15) << "Constraint = " << chup << std::endl;
}
-
/* <constraintId> is the number of the constraint in the constraintH list. */
-void SolverHierarchicalInequalities::
-downdateConstraintHierarchic( const unsigned int kdown )
-{
- ConstraintMem & cdown = constraintH[kdown];
+void SolverHierarchicalInequalities::downdateConstraintHierarchic(
+ const unsigned int kdown) {
+ ConstraintMem &cdown = constraintH[kdown];
sotDEBUG(15) << "kdown = " << kdown << std::endl;
- if(! constraintH[kdown].active) return;
+ if (!constraintH[kdown].active)
+ return;
/* Rh is hessenberg: trigonalize. */
lastRotation.clear();
- for( unsigned int i=cdown.range+1;i<rankh;++i )
- {
- sotRotationSimpleGiven Gr( Rh,i-1,i,i ); Gr.inverse();
- Gr.multiplyRightTranspose(Rh);
- lastRotation.pushBack(Gr);
- }
+ for (unsigned int i = cdown.range + 1; i < rankh; ++i) {
+ sotRotationSimpleGiven Gr(Rh, i - 1, i, i);
+ Gr.inverse();
+ Gr.multiplyRightTranspose(Rh);
+ lastRotation.pushBack(Gr);
+ }
Qh.pushBack(lastRotation);
/* Shift the column after kdown to the left. */
- for( ConstraintList::iterator iter=constraintH.begin();
- iter!=constraintH.end();++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.range>cdown.range ) cs.range--;
- }
+ for (ConstraintList::iterator iter = constraintH.begin();
+ iter != constraintH.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.range > cdown.range)
+ cs.range--;
+ }
/* Remove the columnd <kdown> from Rh. */
- //bubClearMatrix(bub::column(Rh,cdown.range));
- //bubClearMatrix(bub::column(Rh,cdown.range).data());
- //bub::vector_assign_scalar<bub::scalar_assign>(bub::column(Rh,cdown.range),0);
- bub::column(Rh,cdown.range).assign(bub::zero_vector<double>(Rh.size1()));
- bubRemoveColumn(accessRh(),cdown.range);
- rankh--; freeRank++; freeRankChange = +1;
+ // bubClearMatrix(bub::column(Rh,cdown.range));
+ // bubClearMatrix(bub::column(Rh,cdown.range).data());
+ // bub::vector_assign_scalar<bub::scalar_assign>(bub::column(Rh,cdown.range),0);
+ bub::column(Rh, cdown.range).assign(bub::zero_vector<double>(Rh.size1()));
+ bubRemoveColumn(accessRh(), cdown.range);
+ rankh--;
+ freeRank++;
+ freeRankChange = +1;
cdown.active = false;
sotDEBUG(5) << "Rhdown = " << (MATLAB)accessRhConst() << std::endl;
- sotDEBUG(5) << "Qhdown = " << MATLAB(Qh,nJ) << std::endl;
- sotDEBUG(5) << "Qhlast = " << MATLAB(lastRotation,nJ) << std::endl;
+ sotDEBUG(5) << "Qhdown = " << MATLAB(Qh, nJ) << std::endl;
+ sotDEBUG(5) << "Qhlast = " << MATLAB(lastRotation, nJ) << std::endl;
sotDEBUG(5) << "Qhlast = " << lastRotation << std::endl;
/* Check for active rank-def constraint. */
- for( ConstraintList::iterator iter=constraintH.begin();
- iter!=constraintH.end();++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.active && (!cs.rankIncreaser) )
- {
- bubVector QJk(cs.Ji); Qh.multiplyLeft(QJk);
- if(fabs(QJk(rankh))>THRESHOLD_ZERO)
- { /* Update constraint: Qh'Ji' = [ Mh'Ji' mh'Ji' Nh'Ji],
- * with Nh'Ji=0.
- * Simply add the [Mh'Ji' mh'Ji' ] to Rh. */
- sotDEBUG(5) << "No rank loss when downdating [" << kdown
- << "] => " << cs.constraintRow << std::endl;
- bub::matrix_column<bubMatrix > Rhk(Rh,rankh);
- bub::project(Rhk,bub::range(0,rankh+1))
- .assign( bub::project(QJk,bub::range(0,rankh+1)));
- cs.range = rankh; cs.rankIncreaser =true;
- rankh++; freeRank--; freeRankChange = 0;
- break;
- }
- }
+ for (ConstraintList::iterator iter = constraintH.begin();
+ iter != constraintH.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active && (!cs.rankIncreaser)) {
+ bubVector QJk(cs.Ji);
+ Qh.multiplyLeft(QJk);
+ if (fabs(QJk(rankh)) > THRESHOLD_ZERO) { /* Update constraint: Qh'Ji' = [
+ * Mh'Ji' mh'Ji' Nh'Ji], with
+ * Nh'Ji=0. Simply add the
+ * [Mh'Ji' mh'Ji' ] to Rh. */
+ sotDEBUG(5) << "No rank loss when downdating [" << kdown << "] => "
+ << cs.constraintRow << std::endl;
+ bub::matrix_column<bubMatrix> Rhk(Rh, rankh);
+ bub::project(Rhk, bub::range(0, rankh + 1))
+ .assign(bub::project(QJk, bub::range(0, rankh + 1)));
+ cs.range = rankh;
+ cs.rankIncreaser = true;
+ rankh++;
+ freeRank--;
+ freeRankChange = 0;
+ break;
+ }
}
+ }
}
-
/* ---------------------------------------------------------- */
/* <kup> is the number of the constraint in the constraintS list. */
-void SolverHierarchicalInequalities::
-updateConstraintSlack( const unsigned int kup,const ConstraintMem::BoundSideType activeSide )
-{
+void SolverHierarchicalInequalities::updateConstraintSlack(
+ const unsigned int kup, const ConstraintMem::BoundSideType activeSide) {
sotDEBUG(15) << "kup = " << activeSide << kup << std::endl;
- ConstraintMem & csup = constraintS[kup];
+ ConstraintMem &csup = constraintS[kup];
// if( freeRank>0 )
// {
/* Compute the limited Jacobian. */
- bubVector QtJt(nJ); QtJt.assign(csup.Ji);
+ bubVector QtJt(nJ);
+ QtJt.assign(csup.Ji);
Qh.multiplyLeft(QtJt);
- bub::vector_range<bubVector> Ft(QtJt,freerange());
+ bub::vector_range<bubVector> Ft(QtJt, freerange());
sotDEBUG(15) << "QsNhJk = " << (MATLAB)Ft << std::endl;
/* TODO: copy directly ck.Ji in JsU, instead of using the tmp var Ft. */
- bub::matrix_column< bubMatrixQRWide > JskU(QhJsU,sizes);
- bub::project(JskU,rangehs())=bub::project(QtJt,rangehs());
- bub::matrix_column< bubMatrixQRWide > Jsk(QhJs,sizes);
- bub::project(Jsk,rangehs())=bub::project(QtJt,rangehs());
-
- bool rankDef=true;
- if(ranks+rankh<nJ)
- {
- /* Householder transposition of QJk. */
- bub::vector_range<bubVector> Ftdown(QtJt,bub::range(rankh+ranks,nJ));
- double beta;
- double normSignFt // Norm with sign!
- = sotRotationSimpleHouseholder::householderExtraction(Ftdown,beta,THRESHOLD_ZERO);
- sotDEBUG(15) << "hk = " << (MATLAB)Ftdown << std::endl;
- sotDEBUG(15) << "betak = " << beta << std::endl;
- sotDEBUG(15) << "normH = " << normSignFt << std::endl;
-
- /* Add a new column in R. */
- QhJsU(rankh+ranks,sizes) = normSignFt;
- QhJs(rankh+ranks,sizes) = normSignFt;
-
- /* Add a new rotation in Qh. */
- if( fabs(normSignFt)>THRESHOLD_ZERO )
- { /* If Jk is adding information to the current R. */
- /* Add the new householder vector in Q. */
- if(fabs(beta)>THRESHOLD_ZERO)
- { Qh.pushBack( sotRotationSimpleHouseholder(Ftdown,beta)); }
- /* Update information about the constraint. */
- csup.rankIncreaser = true;
- /* Increase the number of data. */
- ranks++; rankDef=false; orderS+=sizes;
- }
+ bub::matrix_column<bubMatrixQRWide> JskU(QhJsU, sizes);
+ bub::project(JskU, rangehs()) = bub::project(QtJt, rangehs());
+ bub::matrix_column<bubMatrixQRWide> Jsk(QhJs, sizes);
+ bub::project(Jsk, rangehs()) = bub::project(QtJt, rangehs());
+
+ bool rankDef = true;
+ if (ranks + rankh < nJ) {
+ /* Householder transposition of QJk. */
+ bub::vector_range<bubVector> Ftdown(QtJt, bub::range(rankh + ranks, nJ));
+ double beta;
+ double normSignFt // Norm with sign!
+ = sotRotationSimpleHouseholder::householderExtraction(Ftdown, beta,
+ THRESHOLD_ZERO);
+ sotDEBUG(15) << "hk = " << (MATLAB)Ftdown << std::endl;
+ sotDEBUG(15) << "betak = " << beta << std::endl;
+ sotDEBUG(15) << "normH = " << normSignFt << std::endl;
+
+ /* Add a new column in R. */
+ QhJsU(rankh + ranks, sizes) = normSignFt;
+ QhJs(rankh + ranks, sizes) = normSignFt;
+
+ /* Add a new rotation in Qh. */
+ if (fabs(normSignFt) >
+ THRESHOLD_ZERO) { /* If Jk is adding information to the current R. */
+ /* Add the new householder vector in Q. */
+ if (fabs(beta) > THRESHOLD_ZERO) {
+ Qh.pushBack(sotRotationSimpleHouseholder(Ftdown, beta));
+ }
+ /* Update information about the constraint. */
+ csup.rankIncreaser = true;
+ /* Increase the number of data. */
+ ranks++;
+ rankDef = false;
+ orderS += sizes;
}
- csup.range = sizes; sizes++;
+ }
+ csup.range = sizes;
+ sizes++;
- sotDEBUG(45) << "QhJs = " << (MATLAB)QhJs << std::endl;
+ sotDEBUG(45) << "QhJs = " << (MATLAB)QhJs << std::endl;
sotDEBUG(45) << "QhJsU = " << (MATLAB)QhJsU << std::endl;
- if( rankDef&&(freeRank>0) )
- { /* The new line does not add any information. Regularize the pseudo-triangle. */
- /* Select the non zero part of R. */
- bubMatrixQR QJdown( QhJsU,freeranges(),bub::range(0,sizes));
- bubMatrixQRTri RrRh(QJdown);
- /* Regularize. */
- sotDEBUG(15) << "RrRh = " << (MATLAB)RrRh << std::endl;
- sotRotationComposed Uchol;
- Uchol.regularizeRankDeficientTriangle(RrRh,orderS,sizes-1);
- csup.rankIncreaser = false;
- bubMatrixQR Jsup(QhJsU,rangeh(),bub::range(0,sizes));
- Uchol.multiplyLeft(Jsup);
- }
- sotDEBUG(15) << "kup = "<<kup<<" VS "<<constraintS.size()<<std::endl;
- csup.active = true; csup.activeSide = activeSide;
+ if (rankDef && (freeRank > 0)) { /* The new line does not add any information.
+ Regularize the pseudo-triangle. */
+ /* Select the non zero part of R. */
+ bubMatrixQR QJdown(QhJsU, freeranges(), bub::range(0, sizes));
+ bubMatrixQRTri RrRh(QJdown);
+ /* Regularize. */
+ sotDEBUG(15) << "RrRh = " << (MATLAB)RrRh << std::endl;
+ sotRotationComposed Uchol;
+ Uchol.regularizeRankDeficientTriangle(RrRh, orderS, sizes - 1);
+ csup.rankIncreaser = false;
+ bubMatrixQR Jsup(QhJsU, rangeh(), bub::range(0, sizes));
+ Uchol.multiplyLeft(Jsup);
+ }
+ sotDEBUG(15) << "kup = " << kup << " VS " << constraintS.size() << std::endl;
+ csup.active = true;
+ csup.activeSide = activeSide;
constraintSactive.push_back(&csup);
sotDEBUG(15) << "Rei = " << (MATLAB)accessRsConst() << std::endl;
sotDEBUG(15) << "Constraint = " << csup << std::endl;
}
/* Regularize from right (Q.J from Q.J.U). */
-void SolverHierarchicalInequalities::
-regularizeQhJs( void )
-{
+void SolverHierarchicalInequalities::regularizeQhJs(void) {
typedef bub::matrix_column<bubMatrixQRWide> bubQJsCol;
sotRotationComposed Qlast;
- for( unsigned int i=0;i<ranks;++i )
- {
- bubQJsCol QJk(QhJs,orderS(i)); Qlast.multiplyLeft(QJk);
- sotDEBUG(45) << "QhJs_" << i << " = " << (MATLAB)QJk << std::endl;
- bub::vector_range<bubQJsCol> Ftdown(QJk,bub::range(rankh+i,std::min(nJ,rankh+ranks+1)));
- double beta;
- double normSignFt
- = sotRotationSimpleHouseholder::householderExtraction(Ftdown,beta,THRESHOLD_ZERO);
- sotDEBUG(45) << "Ft_" << i << " = " << (MATLAB)Ftdown << std::endl;
- sotDEBUG(45) << "b_" << i << " = " << beta << std::endl;
- if(fabs(beta)>THRESHOLD_ZERO)
- { Qlast.pushBack(sotRotationSimpleHouseholder(Ftdown,beta)); }
- std::fill(Ftdown.begin(),Ftdown.end(),0); Ftdown(0)=normSignFt;
+ for (unsigned int i = 0; i < ranks; ++i) {
+ bubQJsCol QJk(QhJs, orderS(i));
+ Qlast.multiplyLeft(QJk);
+ sotDEBUG(45) << "QhJs_" << i << " = " << (MATLAB)QJk << std::endl;
+ bub::vector_range<bubQJsCol> Ftdown(
+ QJk, bub::range(rankh + i, std::min(nJ, rankh + ranks + 1)));
+ double beta;
+ double normSignFt = sotRotationSimpleHouseholder::householderExtraction(
+ Ftdown, beta, THRESHOLD_ZERO);
+ sotDEBUG(45) << "Ft_" << i << " = " << (MATLAB)Ftdown << std::endl;
+ sotDEBUG(45) << "b_" << i << " = " << beta << std::endl;
+ if (fabs(beta) > THRESHOLD_ZERO) {
+ Qlast.pushBack(sotRotationSimpleHouseholder(Ftdown, beta));
}
+ std::fill(Ftdown.begin(), Ftdown.end(), 0);
+ Ftdown(0) = normSignFt;
+ }
Qh.pushBack(Qlast);
- for( ConstraintRefList::iterator iter=constraintSactive.begin();
- iter!=constraintSactive.end();++iter )
- {
- ConstraintMem & cs = **iter;
- if(!cs.rankIncreaser)
- {
- bubQJsCol QJk(QhJs,cs.range);
- Qlast.multiplyLeft(QJk);
- }
+ for (ConstraintRefList::iterator iter = constraintSactive.begin();
+ iter != constraintSactive.end(); ++iter) {
+ ConstraintMem &cs = **iter;
+ if (!cs.rankIncreaser) {
+ bubQJsCol QJk(QhJs, cs.range);
+ Qlast.multiplyLeft(QJk);
}
- sotDEBUG(15)<<"QhJs = " << (MATLAB)QhJs << std::endl;
- sotDEBUG(15)<<"Qreg = " << MATLAB(Qlast,nJ) << std::endl;
+ }
+ sotDEBUG(15) << "QhJs = " << (MATLAB)QhJs << std::endl;
+ sotDEBUG(15) << "Qreg = " << MATLAB(Qlast, nJ) << std::endl;
}
/* Regularize from right (Q.J from Q.J.U). */
-void SolverHierarchicalInequalities::
-regularizeQhJsU( void )
-{
- if( ranks==sizes ) return;
- bubMatrixQR QhJsinf( QhJsU,bub::range(rankh,rankh+ranks),bub::range(0,sizes) );
- bubMatrixQRTri Rrh( QhJsinf );
- bubMatrixQR Jssup(QhJsU,rangeh(),bub::range(0,sizes));
+void SolverHierarchicalInequalities::regularizeQhJsU(void) {
+ if (ranks == sizes)
+ return;
+ bubMatrixQR QhJsinf(QhJsU, bub::range(rankh, rankh + ranks),
+ bub::range(0, sizes));
+ bubMatrixQRTri Rrh(QhJsinf);
+ bubMatrixQR Jssup(QhJsU, rangeh(), bub::range(0, sizes));
sotDEBUG(15) << "Rrh = " << (MATLAB)Rrh << std::endl;
/* Search for the rank def columns. */
- std::vector<bool> rankDefColumns(sizes,true);
- for( unsigned int i=0;i<ranks;++i ) rankDefColumns[orderS(i)]=false;
+ std::vector<bool> rankDefColumns(sizes, true);
+ for (unsigned int i = 0; i < ranks; ++i)
+ rankDefColumns[orderS(i)] = false;
/* Regularize the rank def cols. */
- for( unsigned int i=0;i<sizes;++i )
- if( rankDefColumns[i] )
- {
- sotRotationComposed Gr; Gr.regularizeRankDeficientTriangle(Rrh,orderS,i);
- Gr.multiplyLeft(Jssup);
- }
+ for (unsigned int i = 0; i < sizes; ++i)
+ if (rankDefColumns[i]) {
+ sotRotationComposed Gr;
+ Gr.regularizeRankDeficientTriangle(Rrh, orderS, i);
+ Gr.multiplyLeft(Jssup);
+ }
sotDEBUG(45) << "R0 = " << (MATLAB)Rrh << std::endl;
- sotDEBUG(15) << "QhJsU = " << (MATLAB)bub::subrange(QhJsU,0,nJ,0,sizes) << std::endl;
+ sotDEBUG(15) << "QhJsU = " << (MATLAB)bub::subrange(QhJsU, 0, nJ, 0, sizes)
+ << std::endl;
}
-
-
/* <constraintId> is the number of the constraint in the constraintS list. */
-void SolverHierarchicalInequalities::
-downdateConstraintSlack( const unsigned int kdown )
-{
+void SolverHierarchicalInequalities::downdateConstraintSlack(
+ const unsigned int kdown) {
/* Remove the column. */
- ConstraintMem & cs = constraintS[kdown];
+ ConstraintMem &cs = constraintS[kdown];
sotDEBUG(15) << "Downdate S " << cs;
const unsigned int range = cs.range;
- if( orderS.end()!=std::find(orderS.begin(),orderS.end(),range) )
- { orderS-=range; ranks--; }
- bubRemoveColumn(QhJs,range); sizes--;
- cs.active=false;
- constraintSactive.erase(constraintSactive.begin()+range);
- for( unsigned int i=0;i<ranks;++i ) if( orderS(i)>range ) orderS(i)--;
- for( ConstraintRefList::iterator iter=constraintSactive.begin();
- iter!=constraintSactive.end();++iter )
- {
- if((*iter)->range>range) (*iter)->range--;
- sotDEBUG(15) << "Remains " << **iter << std::endl;
- }
+ if (orderS.end() != std::find(orderS.begin(), orderS.end(), range)) {
+ orderS -= range;
+ ranks--;
+ }
+ bubRemoveColumn(QhJs, range);
+ sizes--;
+ cs.active = false;
+ constraintSactive.erase(constraintSactive.begin() + range);
+ for (unsigned int i = 0; i < ranks; ++i)
+ if (orderS(i) > range)
+ orderS(i)--;
+ for (ConstraintRefList::iterator iter = constraintSactive.begin();
+ iter != constraintSactive.end(); ++iter) {
+ if ((*iter)->range > range)
+ (*iter)->range--;
+ sotDEBUG(15) << "Remains " << **iter << std::endl;
+ }
/* Triangularize the remaining QhJs matrix. */
regularizeQhJs();
/* Check for rank update. */
- for( unsigned int i=0;i<sizes;++i )
- {
- sotDEBUG(45) << "At i=" << i << ",j="<<rankh+ranks
- <<": qj=" << QhJs(rankh+ranks,i) << std::endl;
- if( (freeRank>ranks)&&(fabs(QhJs(rankh+ranks,i))>THRESHOLD_ZERO) )
- {
- sotDEBUG(15) << "No ranks lost when downdating S." << std::endl;
- orderS+=i; ranks++; constraintSactive[i]->rankIncreaser=true;
- break;
- }
+ for (unsigned int i = 0; i < sizes; ++i) {
+ sotDEBUG(45) << "At i=" << i << ",j=" << rankh + ranks
+ << ": qj=" << QhJs(rankh + ranks, i) << std::endl;
+ if ((freeRank > ranks) && (fabs(QhJs(rankh + ranks, i)) > THRESHOLD_ZERO)) {
+ sotDEBUG(15) << "No ranks lost when downdating S." << std::endl;
+ orderS += i;
+ ranks++;
+ constraintSactive[i]->rankIncreaser = true;
+ break;
}
+ }
- bub::project(QhJsU,bub::range(0,nJ),bub::range(0,sizes))
- .assign(bub::project(QhJs,bub::range(0,nJ),bub::range(0,sizes)));
- bub::project(QhJs,bub::range(0,nJ),bub::range(sizes,QhJsU.size2()))
- .assign(bub::zero_matrix<double>(nJ,QhJsU.size2()-sizes));
- bub::project(QhJsU,bub::range(0,nJ),bub::range(sizes,QhJsU.size2()))
- .assign(bub::zero_matrix<double>(nJ,QhJsU.size2()-sizes));
+ bub::project(QhJsU, bub::range(0, nJ), bub::range(0, sizes))
+ .assign(bub::project(QhJs, bub::range(0, nJ), bub::range(0, sizes)));
+ bub::project(QhJs, bub::range(0, nJ), bub::range(sizes, QhJsU.size2()))
+ .assign(bub::zero_matrix<double>(nJ, QhJsU.size2() - sizes));
+ bub::project(QhJsU, bub::range(0, nJ), bub::range(sizes, QhJsU.size2()))
+ .assign(bub::zero_matrix<double>(nJ, QhJsU.size2() - sizes));
regularizeQhJsU();
// std::cerr << "Not implemented yet (RotationSimple l" << __LINE__
@@ -1408,123 +1443,117 @@ downdateConstraintSlack( const unsigned int kdown )
// throw "Not implemented yet.";
}
-
/* ---------------------------------------------------------- */
-void SolverHierarchicalInequalities::
-updateRankOneDowndate( void )
-{
- sotDEBUG(15) << "/* Apply the last corrections of Qh. */"<<std::endl;
- bubMatrixQR QhJranksU(QhJsU,fullrange(),bub::range(0,sizes));
+void SolverHierarchicalInequalities::updateRankOneDowndate(void) {
+ sotDEBUG(15) << "/* Apply the last corrections of Qh. */" << std::endl;
+ bubMatrixQR QhJranksU(QhJsU, fullrange(), bub::range(0, sizes));
lastRotation.multiplyRightTranspose(QhJranksU);
- bubMatrixQR QhJranks(QhJs,fullrange(),bub::range(0,sizes));
+ bubMatrixQR QhJranks(QhJs, fullrange(), bub::range(0, sizes));
lastRotation.multiplyRightTranspose(QhJranks);
- sotDEBUG(15) << "/* Check for the full rank of Rs. */"<<std::endl;
- sotDEBUG(25) << "beforeQhJsU = " << (MATLAB)bub::subrange(QhJsU,0,nJ,0,sizes) << std::endl;
- sotDEBUG(25) << "beforeQhJs = " << (MATLAB)bub::subrange(QhJs,0,nJ,0,sizes) << std::endl;
- bubMatrixQR Rsno(QhJsU,bub::range(rankh,std::min(nJ,rankh+ranks)),bub::range(0,sizes));
- for( unsigned int i=0;i<ranks;++i )
- {
- const unsigned int col = orderS[i];
- if( (i>=freeRank)||(fabs(Rsno(i,col))<THRESHOLD_ZERO) )
- {
- sotDEBUG(5) << "Rank lost at " << i << "." << std::endl;
- orderS-=col; ranks--;
- constraintSactive[col]->rankIncreaser = false;
- if( i>0 )
- {
- sotRotationComposed Gr;
- bubMatrixQR
- Ri(QhJsU,bub::range(rankh,std::min(nJ,rankh+i)),bub::range(0,sizes));
- Gr.regularizeRankDeficientTriangle(Ri,orderS,col);
- bubMatrixQR Jsup(QhJsU,rangeh(),bub::range(0,sizes));
- Gr.multiplyLeft(Jsup);
- }
-
- break;
- }
+ sotDEBUG(15) << "/* Check for the full rank of Rs. */" << std::endl;
+ sotDEBUG(25) << "beforeQhJsU = "
+ << (MATLAB)bub::subrange(QhJsU, 0, nJ, 0, sizes) << std::endl;
+ sotDEBUG(25) << "beforeQhJs = "
+ << (MATLAB)bub::subrange(QhJs, 0, nJ, 0, sizes) << std::endl;
+ bubMatrixQR Rsno(QhJsU, bub::range(rankh, std::min(nJ, rankh + ranks)),
+ bub::range(0, sizes));
+ for (unsigned int i = 0; i < ranks; ++i) {
+ const unsigned int col = orderS[i];
+ if ((i >= freeRank) || (fabs(Rsno(i, col)) < THRESHOLD_ZERO)) {
+ sotDEBUG(5) << "Rank lost at " << i << "." << std::endl;
+ orderS -= col;
+ ranks--;
+ constraintSactive[col]->rankIncreaser = false;
+ if (i > 0) {
+ sotRotationComposed Gr;
+ bubMatrixQR Ri(QhJsU, bub::range(rankh, std::min(nJ, rankh + i)),
+ bub::range(0, sizes));
+ Gr.regularizeRankDeficientTriangle(Ri, orderS, col);
+ bubMatrixQR Jsup(QhJsU, rangeh(), bub::range(0, sizes));
+ Gr.multiplyLeft(Jsup);
+ }
+
+ break;
}
- sotDEBUG(5) << "unrQhJsU = "<< (MATLAB)QhJsU << std::endl;
- sotDEBUG(15) << "/* Regularize the Hessenberg matrix. */"<<std::endl;
- for( unsigned int i=0;i<ranks;++i )
- {
- const unsigned int col = orderS[i];
- if( (i+1<freeRank)&&(fabsf(QhJsU(rankh+i+1,col))>THRESHOLD_ZERO) )
- {
- sotRotationSimpleGiven Gr( QhJsU,rankh+i,rankh+i+1,col ); Gr.inverse();
- Gr.multiplyRightTranspose(QhJsU);
- Gr.multiplyRightTranspose(QhJs);
- Qh.pushBack(Gr);
- }
- sotDEBUG(45) << "QhJs = " << (MATLAB)QhJs << std::endl;
- sotDEBUG(15) << "QhJsU = " << (MATLAB)QhJsU << std::endl;
- sotDEBUG(5) << "Rs = " << (MATLAB)accessRsConst() << std::endl;
+ }
+ sotDEBUG(5) << "unrQhJsU = " << (MATLAB)QhJsU << std::endl;
+ sotDEBUG(15) << "/* Regularize the Hessenberg matrix. */" << std::endl;
+ for (unsigned int i = 0; i < ranks; ++i) {
+ const unsigned int col = orderS[i];
+ if ((i + 1 < freeRank) &&
+ (fabsf(QhJsU(rankh + i + 1, col)) > THRESHOLD_ZERO)) {
+ sotRotationSimpleGiven Gr(QhJsU, rankh + i, rankh + i + 1, col);
+ Gr.inverse();
+ Gr.multiplyRightTranspose(QhJsU);
+ Gr.multiplyRightTranspose(QhJs);
+ Qh.pushBack(Gr);
}
+ sotDEBUG(45) << "QhJs = " << (MATLAB)QhJs << std::endl;
+ sotDEBUG(15) << "QhJsU = " << (MATLAB)QhJsU << std::endl;
+ sotDEBUG(5) << "Rs = " << (MATLAB)accessRsConst() << std::endl;
+ }
}
-void SolverHierarchicalInequalities::
-updateRankOneUpdate( void )
-{
+void SolverHierarchicalInequalities::updateRankOneUpdate(void) {
sotDEBUG(15) << "ranks = " << ranks << std::endl;
- sotDEBUG(15) << "/* Apply the last corrections of Qh. */"<<std::endl;
- bubMatrixQR QhJranksU(QhJsU,fullrange(),bub::range(0,sizes));
+ sotDEBUG(15) << "/* Apply the last corrections of Qh. */" << std::endl;
+ bubMatrixQR QhJranksU(QhJsU, fullrange(), bub::range(0, sizes));
sotDEBUG(25) << "beforeQhJsU = " << (MATLAB)QhJranksU << std::endl;
lastRotation.multiplyRightTranspose(QhJranksU);
- bubMatrixQR QhJranks(QhJs,fullrange(),bub::range(0,sizes));
+ bubMatrixQR QhJranks(QhJs, fullrange(), bub::range(0, sizes));
sotDEBUG(25) << "beforeQhJs = " << (MATLAB)QhJranks << std::endl;
lastRotation.multiplyRightTranspose(QhJranks);
sotDEBUG(25) << "hessenbergQhJsU = " << (MATLAB)QhJranksU << std::endl;
sotDEBUG(25) << "order = " << (MATLAB)orderS << std::endl;
- sotDEBUG(15) << "/* Regularize the Hessenberg matrix. */"<<std::endl;
- std::vector<bool> notFullRank(sizes,true);
+ sotDEBUG(15) << "/* Regularize the Hessenberg matrix. */" << std::endl;
+ std::vector<bool> notFullRank(sizes, true);
sotRotationComposed Qhdebug;
- for( unsigned int i=0;i<ranks;++i )
- {
- const unsigned int col = orderS[i];
- if((i+1<freeRank)&&(fabs(QhJsU(rankh+i+1,col))>THRESHOLD_ZERO))
- {
- sotDEBUG(45) << "Regularize diag-up on " << col << ". " << std::endl;
- sotRotationSimpleGiven Gr( QhJsU,rankh+i,rankh+i+1,col ); Gr.inverse();
- Gr.multiplyRightTranspose(QhJsU);
- Gr.multiplyRightTranspose(QhJs);
- Qh.pushBack(Gr);
- Qhdebug.pushBack(Gr);
- }
- notFullRank[col]=false;
+ for (unsigned int i = 0; i < ranks; ++i) {
+ const unsigned int col = orderS[i];
+ if ((i + 1 < freeRank) &&
+ (fabs(QhJsU(rankh + i + 1, col)) > THRESHOLD_ZERO)) {
+ sotDEBUG(45) << "Regularize diag-up on " << col << ". " << std::endl;
+ sotRotationSimpleGiven Gr(QhJsU, rankh + i, rankh + i + 1, col);
+ Gr.inverse();
+ Gr.multiplyRightTranspose(QhJsU);
+ Gr.multiplyRightTranspose(QhJs);
+ Qh.pushBack(Gr);
+ Qhdebug.pushBack(Gr);
}
+ notFullRank[col] = false;
+ }
- sotDEBUG(25) << "Qhmodif = " << MATLAB(Qhdebug,nJ) << std::endl;
- if( freeRank>0 )
- {
- sotDEBUG(25) << "regQhJsU = " << (MATLAB)QhJranksU << std::endl;
- sotDEBUG(25) << "regQhJs = " << (MATLAB)QhJranks << std::endl;
- sotDEBUG(15) << "/* Check for the rank update of Rs. */"<<std::endl;
- bool rankOneUpdateDone = false;
- for( unsigned int i=0;i<sizes;++i )
- {
- bubMatrixQR Rsno(QhJsU,bub::range(rankh,rankh+ranks),bub::range(0,sizes));
- if( notFullRank[i] )
- {
- /* Check that last-row value is still null. */
- if( (!rankOneUpdateDone)&&(rankh+ranks<nJ)
- &&(fabs(QhJsU(rankh+ranks,i))>THRESHOLD_ZERO) )
- {
- sotDEBUG(5) << "Rank inc at " << i << "." << std::endl;
- ranks++; orderS+=i; rankOneUpdateDone=true;;
- constraintSactive[i]->rankIncreaser = true;
- }
- else if( orderS.size()>0)
- {
- sotDEBUG(5)<<"ranks_after = " << ranks <<std::endl;
- sotRotationComposed U;
- U.regularizeRankDeficientTriangle(Rsno,orderS,i);
- bubMatrixQR Jsup(QhJsU,rangeh(),bub::range(0,sizes));
- U.multiplyLeft(Jsup);
- }
- }
+ sotDEBUG(25) << "Qhmodif = " << MATLAB(Qhdebug, nJ) << std::endl;
+ if (freeRank > 0) {
+ sotDEBUG(25) << "regQhJsU = " << (MATLAB)QhJranksU << std::endl;
+ sotDEBUG(25) << "regQhJs = " << (MATLAB)QhJranks << std::endl;
+ sotDEBUG(15) << "/* Check for the rank update of Rs. */" << std::endl;
+ bool rankOneUpdateDone = false;
+ for (unsigned int i = 0; i < sizes; ++i) {
+ bubMatrixQR Rsno(QhJsU, bub::range(rankh, rankh + ranks),
+ bub::range(0, sizes));
+ if (notFullRank[i]) {
+ /* Check that last-row value is still null. */
+ if ((!rankOneUpdateDone) && (rankh + ranks < nJ) &&
+ (fabs(QhJsU(rankh + ranks, i)) > THRESHOLD_ZERO)) {
+ sotDEBUG(5) << "Rank inc at " << i << "." << std::endl;
+ ranks++;
+ orderS += i;
+ rankOneUpdateDone = true;
+ ;
+ constraintSactive[i]->rankIncreaser = true;
+ } else if (orderS.size() > 0) {
+ sotDEBUG(5) << "ranks_after = " << ranks << std::endl;
+ sotRotationComposed U;
+ U.regularizeRankDeficientTriangle(Rsno, orderS, i);
+ bubMatrixQR Jsup(QhJsU, rangeh(), bub::range(0, sizes));
+ U.multiplyLeft(Jsup);
}
+ }
}
+ }
sotDEBUG(45) << "QhJs = " << (MATLAB)QhJs << std::endl;
sotDEBUG(15) << "QhJsU = " << (MATLAB)QhJsU << std::endl;
sotDEBUG(5) << "Rs = " << (MATLAB)accessRsConst() << std::endl;
@@ -1540,25 +1569,27 @@ updateRankOneUpdate( void )
// lastRotation.multiplyRightTranspose(QhJranks);
// sotDEBUG(15) << "/* Check for the full rank of Rs. */"<<std::endl;
-// sotDEBUG(25) << "beforeQhJsU = " << (MATLAB)bub::subrange(QhJsU,0,nJ,0,sizes) << std::endl;
-// /* rpc contains a map giving for each rank=1:sizes the ref of the column of that size. */
-// std::vector< std::list<unsigned int> > rankPerColumn(sizes);
-// sotDEBUG(15) << "/* Classify the columns per rank in <rankPerColumn>. */"<<std::endl;
-// for( unsigned int col=0;col<sizes;++col )
+// sotDEBUG(25) << "beforeQhJsU = " <<
+// (MATLAB)bub::subrange(QhJsU,0,nJ,0,sizes) << std::endl;
+// /* rpc contains a map giving for each rank=1:sizes the ref of the column
+// of that size. */ std::vector< std::list<unsigned int> >
+// rankPerColumn(sizes); sotDEBUG(15) << "/* Classify the columns per rank
+// in <rankPerColumn>. */"<<std::endl; for( unsigned int
+// col=0;col<sizes;++col )
// {
// for( unsigned int row=nJ-1;row>=rankh;--row )
// {
-// sotDEBUG(50) << row << " x " << col << " = " << fabs(QhJsU(row,col)) << std::endl;
+// sotDEBUG(50) << row << " x " << col << " = " <<
+// fabs(QhJsU(row,col)) << std::endl;
// /* Search for the first non-zero. */
// if( fabs(QhJsU(row,col))>THRESHOLD_ZERO )
// { rankPerColumn[row-rankh].push_back(col); break; }
// }
// }
-// sotDEBUG(15) << "/* Select the columns to shape a triangle. */"<<std::endl;
-// bub::unbounded_array<std::size_t> columnOrder(sizes); ranks=0;
-// std::list<unsigned int> needRightRegularization;
-// bool needLeftRegularization = false;
-// for( unsigned int i=0;i<sizes;++i )
+// sotDEBUG(15) << "/* Select the columns to shape a triangle.
+// */"<<std::endl; bub::unbounded_array<std::size_t> columnOrder(sizes);
+// ranks=0; std::list<unsigned int> needRightRegularization; bool
+// needLeftRegularization = false; for( unsigned int i=0;i<sizes;++i )
// {
// sotDEBUG(45) << i << std::endl;
// std::list<unsigned int> & rpci = rankPerColumn[i];
@@ -1569,11 +1600,12 @@ updateRankOneUpdate( void )
// }
// else
// {
-// sotDEBUG(5) << "Need left regularization for col " << i << std::endl;
-// needLeftRegularization = true;
+// sotDEBUG(5) << "Need left regularization for col " << i <<
+// std::endl; needLeftRegularization = true;
// }
// if( rpci.size()>0 )
-// needRightRegularization.insert(needRightRegularization.end(), rpci.begin(), rpci.end());
+// needRightRegularization.insert(needRightRegularization.end(),
+// rpci.begin(), rpci.end());
// }
// orderS = bubOrder(ranks,columnOrder);
// sotDEBUG(5) << "unrQhJsU = "<< (MATLAB)accessQRs() << std::endl;
@@ -1582,20 +1614,22 @@ updateRankOneUpdate( void )
// sotDEBUG(15) << "/* Regularize the Hessenberg matrix. */"<<std::endl;
// for( unsigned int i=0;i<ranks;++i )
// {
-// sotRotationSimpleGiven Gr( QhJsU,rankh+i,rankh+i+1,columnOrder[i] ); Gr.inverse();
-// Gr.multiplyRightTranspose(QhJsU);
+// sotRotationSimpleGiven Gr( QhJsU,rankh+i,rankh+i+1,columnOrder[i]
+// ); Gr.inverse(); Gr.multiplyRightTranspose(QhJsU);
// Gr.multiplyRightTranspose(QhJs);
// Qh.pushBack(Gr);
// }
// }
// sotDEBUG(5) << "rlQhJsU = "<< (MATLAB)QhJsU << std::endl;
// sotDEBUG(15) << "/* Regularize the rank-def columns. */"<<std::endl;
-// bubMatrixQR QhJsUdown( QhJsU,bub::range(rankh,rankh+ranks),bub::range(0,sizes));
-// for( std::list<unsigned int>::iterator iter=needRightRegularization.begin();
+// bubMatrixQR QhJsUdown(
+// QhJsU,bub::range(rankh,rankh+ranks),bub::range(0,sizes)); for(
+// std::list<unsigned int>::iterator iter=needRightRegularization.begin();
// iter!=needRightRegularization.end();++iter )
// {
// sotDEBUG(25) << "Regularize right " << *iter << std::endl;
-// sotRotationComposed Gr; Gr.regularizeRankDeficientTriangle(QhJsUdown,orderS,*iter);
+// sotRotationComposed Gr;
+// Gr.regularizeRankDeficientTriangle(QhJsUdown,orderS,*iter);
// }
// sotDEBUG(45) << "QhJs = " << (MATLAB)QhJs << std::endl;
// sotDEBUG(15) << "QhJsU = " << (MATLAB)QhJsU << std::endl;
@@ -1607,318 +1641,330 @@ updateRankOneUpdate( void )
* of the active slack constraints.
* <gradient> must be of size <nJ> (no resize).
*/
-void SolverHierarchicalInequalities::
-computeGradient( bubVector& gradientWide )
-{
+void SolverHierarchicalInequalities::computeGradient(bubVector &gradientWide) {
// bubVector gse(ese.size()); gse=ese;
// bub::axpy_prod( Jse,-u0,gse,false ); /* gse = ese - Jse u0 */
- // bub::axpy_prod( gse,Jse,gradientWide,true ); /* g = J' (ese - Jse u0 ) */
- gradientWide .clear();
- for( ConstraintRefList::const_iterator iter=constraintSactive.begin();
- iter!=constraintSactive.end();++iter )
- {
- const ConstraintMem & ci = **iter;
- const double ciei = (ci.activeSide==ConstraintMem::BOUND_INF)?ci.eiInf:ci.eiSup;
- double epJu = ciei - bub::inner_prod(ci.Ji,u0);
- gradientWide += epJu*ci.Ji;
- sotDEBUG(55) << "Gradient with " << ci << " -> g = " << (MATLAB)gradientWide << std::endl;
- }
+ // bub::axpy_prod( gse,Jse,gradientWide,true ); /* g = J' (ese - Jse u0 )
+ // */
+ gradientWide.clear();
+ for (ConstraintRefList::const_iterator iter = constraintSactive.begin();
+ iter != constraintSactive.end(); ++iter) {
+ const ConstraintMem &ci = **iter;
+ const double ciei =
+ (ci.activeSide == ConstraintMem::BOUND_INF) ? ci.eiInf : ci.eiSup;
+ double epJu = ciei - bub::inner_prod(ci.Ji, u0);
+ gradientWide += epJu * ci.Ji;
+ sotDEBUG(55) << "Gradient with " << ci << " -> g = " << (MATLAB)gradientWide
+ << std::endl;
+ }
}
/* Compute the primal solution of the (equality-only) QP problem,
* using the Null-Space method:
* du = Nh Ms Rs^-T Rs^-1 Ms' Nh' Js' ( es - Js u0 )
*/
-void SolverHierarchicalInequalities::
-computePrimal( void )
-{
- if( (0==freeRank)||(0==ranks) )
- {
- sotDEBUG(15) <<"Ranks null, du is 0." << std::endl;
- du.resize(nJ,false); du.clear();
- return;
- }
+void SolverHierarchicalInequalities::computePrimal(void) {
+ if ((0 == freeRank) || (0 == ranks)) {
+ sotDEBUG(15) << "Ranks null, du is 0." << std::endl;
+ du.resize(nJ, false);
+ du.clear();
+ return;
+ }
const bubMatrixQROrderedTriConst Rei = accessRsConst();
sotDEBUG(15) << "Rei = " << (MATLAB)Rei << std::endl;
- sotDEBUG(15) << "/* du <- Js' ( es - Js u0 ) */"<< std::endl;
- du.resize(nJ,false); computeGradient(du);
+ sotDEBUG(15) << "/* du <- Js' ( es - Js u0 ) */" << std::endl;
+ du.resize(nJ, false);
+ computeGradient(du);
sotDEBUG(15) << "Jtei = " << (MATLAB)du << std::endl;
- sotDEBUG(15) << "/* Mbei <- Ms' Nh' Js' ( es - Js u0 ) */"<< std::endl;
- bub::vector_range<bubVector> Mbei
- = Qh.multiplyRangeLeft(du,rankh,freeRank-ranks);
- sotDEBUG(15) << "--"<< std::endl;
+ sotDEBUG(15) << "/* Mbei <- Ms' Nh' Js' ( es - Js u0 ) */" << std::endl;
+ bub::vector_range<bubVector> Mbei =
+ Qh.multiplyRangeLeft(du, rankh, freeRank - ranks);
+ sotDEBUG(15) << "--" << std::endl;
sotDEBUG(45) << "Qbei = " << (MATLAB)du << std::endl;
sotDEBUG(15) << "Mbei = " << (MATLAB)Mbei << std::endl;
- sotDEBUG(15) << "/* Mbei <- R-ei Ms' Jt' et */"<< std::endl;
- bub::lu_substitute(Rei,Mbei);
+ sotDEBUG(15) << "/* Mbei <- R-ei Ms' Jt' et */" << std::endl;
+ bub::lu_substitute(Rei, Mbei);
sotDEBUG(15) << "RMbei = " << (MATLAB)Mbei << std::endl;
- sotDEBUG(15) << "/* Mbei <- R'-1 R-1 Ms' Jt' et */"<< std::endl;
- bub::lu_substitute(Mbei,(const bubMatrix &)Rei);
+ sotDEBUG(15) << "/* Mbei <- R'-1 R-1 Ms' Jt' et */" << std::endl;
+ bub::lu_substitute(Mbei, (const bubMatrix &)Rei);
sotDEBUG(15) << "RRMbei = " << (MATLAB)Mbei << std::endl;
- sotDEBUG(15) << "/* du <- Qh * [ 0 ; Qs * [ Mbei; 0 ] ] */"<< std::endl;
- Qh.multiplyRangeRight(du,rankh,(freeRank-ranks));
+ sotDEBUG(15) << "/* du <- Qh * [ 0 ; Qs * [ Mbei; 0 ] ] */" << std::endl;
+ Qh.multiplyRangeRight(du, rankh, (freeRank - ranks));
sotDEBUG(5) << "duei = " << (MATLAB)du << std::endl;
}
/* ---------------------------------------------------------- */
/* Compute the slack w = es-Js(u0+du). Since esi<Jsi.u, the slack
* w = esi-Jsi.u should be negative. Since Jss.u<ess, the slack
* w = Jss.u-ess should also be negative. */
-void SolverHierarchicalInequalities::
-computeSlack( void )
-{
- slackInf.resize(constraintS.size(),false); slackInf.clear();
- slackSup.resize(constraintS.size(),false); slackSup.clear();
- bubVector updu(nJ); updu=u0; // updu+=du;
+void SolverHierarchicalInequalities::computeSlack(void) {
+ slackInf.resize(constraintS.size(), false);
+ slackInf.clear();
+ slackSup.resize(constraintS.size(), false);
+ slackSup.clear();
+ bubVector updu(nJ);
+ updu = u0; // updu+=du;
sotDEBUG(5) << "updu = " << (MATLAB)updu << std::endl;
- for( ConstraintList::const_iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter )
- {
- const ConstraintMem & cs = *iter;
- if(! cs.equality)
- {
- const double Ju = bub::inner_prod(cs.Ji,updu);
- sotDEBUG(55) << "J = " << (MATLAB)cs.Ji << std::endl;
- sotDEBUG(55) << "Ju = " << Ju << std::endl;
-
- if( cs.boundSide&ConstraintMem::BOUND_INF )
- { slackInf(cs.constraintRow) = cs.eiInf - Ju; }
- else { slackInf(cs.constraintRow) = 0; }
- if( cs.boundSide&ConstraintMem::BOUND_SUP )
- { slackSup(cs.constraintRow) = Ju - cs.eiSup; }
- else { slackSup(cs.constraintRow) = 0; }
- }
+ for (ConstraintList::const_iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter) {
+ const ConstraintMem &cs = *iter;
+ if (!cs.equality) {
+ const double Ju = bub::inner_prod(cs.Ji, updu);
+ sotDEBUG(55) << "J = " << (MATLAB)cs.Ji << std::endl;
+ sotDEBUG(55) << "Ju = " << Ju << std::endl;
+
+ if (cs.boundSide & ConstraintMem::BOUND_INF) {
+ slackInf(cs.constraintRow) = cs.eiInf - Ju;
+ } else {
+ slackInf(cs.constraintRow) = 0;
+ }
+ if (cs.boundSide & ConstraintMem::BOUND_SUP) {
+ slackSup(cs.constraintRow) = Ju - cs.eiSup;
+ } else {
+ slackSup(cs.constraintRow) = 0;
+ }
}
+ }
sotDEBUG(5) << "slackInf = " << (MATLAB)slackInf << std::endl;
sotDEBUG(5) << "slackSup = " << (MATLAB)slackSup << std::endl;
}
-void SolverHierarchicalInequalities::
-computeLagrangian( void )
-{
- if(rankh==0) return;
- lagrangian.resize(rankh,false); lagrangian.clear();
+void SolverHierarchicalInequalities::computeLagrangian(void) {
+ if (rankh == 0)
+ return;
+ lagrangian.resize(rankh, false);
+ lagrangian.clear();
/* bs <- Js' (es - Js.u) */
- bubVector bs(nJ); computeGradient(bs);
+ bubVector bs(nJ);
+ computeGradient(bs);
sotDEBUG(15) << "bs = " << (MATLAB)bs << std::endl;
/* bs <- Qh' Js' (es - Js.u) */
/* ms <- Mh' Js' (es - Js.u) */
- bub::vector_range<bubVector> Mbs = Qh.multiplyRangeLeft(bs,0,freeRank);
+ bub::vector_range<bubVector> Mbs = Qh.multiplyRangeLeft(bs, 0, freeRank);
sotDEBUG(45) << "Qbs = " << (MATLAB)bs << std::endl;
sotDEBUG(45) << "Mbs = " << (MATLAB)Mbs << std::endl;
Mbs *= -1;
/* Mbs <- R0^-1 Mh' Js' (-es + Js.u) */
sotDEBUG(45) << "Rh = " << (MATLAB)accessRhConst() << std::endl;
- bub::lu_substitute(accessRhConst(),Mbs);
+ bub::lu_substitute(accessRhConst(), Mbs);
sotDEBUG(45) << "RMbs = " << (MATLAB)Mbs << std::endl;
- lagrangian.resize(Mbs.size(),false);
- for( ConstraintList::iterator iter=constraintH.begin();
- iter!=constraintH.end();++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.active && cs.rankIncreaser )
- {
- const unsigned int range = cs.range;
- if( cs.activeSide&ConstraintMem::BOUND_INF )
- { lagrangian(range) = Mbs(range); }
- else if( cs.activeSide&ConstraintMem::BOUND_SUP )
- { lagrangian(range) = -Mbs(range); }
- }
+ lagrangian.resize(Mbs.size(), false);
+ for (ConstraintList::iterator iter = constraintH.begin();
+ iter != constraintH.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active && cs.rankIncreaser) {
+ const unsigned int range = cs.range;
+ if (cs.activeSide & ConstraintMem::BOUND_INF) {
+ lagrangian(range) = Mbs(range);
+ } else if (cs.activeSide & ConstraintMem::BOUND_SUP) {
+ lagrangian(range) = -Mbs(range);
+ }
}
- //lagrangian.assign(Mbs);
+ }
+ // lagrangian.assign(Mbs);
sotDEBUG(5) << "lagrangian = " << (MATLAB)lagrangian << std::endl;
}
-
/* ---------------------------------------------------------- */
-bool SolverHierarchicalInequalities::
-selecActivationHierarchic( double & tau )
-{
- tau=1-THRESHOLD_ZERO; unsigned int constraintRef = 0;
+bool SolverHierarchicalInequalities::selecActivationHierarchic(double &tau) {
+ tau = 1 - THRESHOLD_ZERO;
+ unsigned int constraintRef = 0;
bool res = false;
- for( ConstraintList::iterator iter=constraintH.begin();
- iter!=constraintH.end();++iter,++constraintRef )
- {
- ConstraintMem & cs = *iter;
- if(!(cs.active||cs.notToBeConsidered))
+ for (ConstraintList::iterator iter = constraintH.begin();
+ iter != constraintH.end(); ++iter, ++constraintRef) {
+ ConstraintMem &cs = *iter;
+ if (!(cs.active || cs.notToBeConsidered)) {
+ cs.Ju = bub::inner_prod(cs.Ji, u0);
+ cs.Jdu = bub::inner_prod(cs.Ji, du);
+ /* Activation Inf. */
+ if (cs.Jdu < -THRESHOLD_ZERO &&
+ (cs.boundSide & ConstraintMem::BOUND_INF)) {
+ const double taui = (cs.eiInf - cs.Ju) / cs.Jdu;
+ if (taui < tau) // If not: activate i.
{
- cs.Ju = bub::inner_prod(cs.Ji,u0);
- cs.Jdu = bub::inner_prod(cs.Ji,du);
- /* Activation Inf. */
- if( cs.Jdu<-THRESHOLD_ZERO&&(cs.boundSide&ConstraintMem::BOUND_INF) )
- {
- const double taui = (cs.eiInf-cs.Ju)/cs.Jdu;
- if( taui<tau ) // If not: activate i.
- { tau=taui; HactivationRef=constraintRef;
- HactivationSide=ConstraintMem::BOUND_INF; res=true; }
- }
- /* Activation Sup. */
- if( cs.Jdu>THRESHOLD_ZERO&&(cs.boundSide&ConstraintMem::BOUND_SUP) )
- {
- const double taui = (cs.eiSup-cs.Ju)/cs.Jdu;
- if( taui<tau ) // If not: activate i.
- { tau=taui; HactivationRef=constraintRef;
- HactivationSide=ConstraintMem::BOUND_SUP; res=true; }
- }
+ tau = taui;
+ HactivationRef = constraintRef;
+ HactivationSide = ConstraintMem::BOUND_INF;
+ res = true;
}
+ }
+ /* Activation Sup. */
+ if (cs.Jdu > THRESHOLD_ZERO &&
+ (cs.boundSide & ConstraintMem::BOUND_SUP)) {
+ const double taui = (cs.eiSup - cs.Ju) / cs.Jdu;
+ if (taui < tau) // If not: activate i.
+ {
+ tau = taui;
+ HactivationRef = constraintRef;
+ HactivationSide = ConstraintMem::BOUND_SUP;
+ res = true;
+ }
+ }
}
- if(res) {sotDEBUG(1) << "Activation H <" << HactivationSide
- << HactivationRef << ">" << std::endl;}
+ }
+ if (res) {
+ sotDEBUG(1) << "Activation H <" << HactivationSide << HactivationRef << ">"
+ << std::endl;
+ }
return res;
}
-bool SolverHierarchicalInequalities::
-selecInactivationHierarchic( void )
-{
- bool res=false; double HinactivationScore=-THRESHOLD_ZERO;
- for( ConstraintList::iterator iter=constraintH.begin();
- iter!=constraintH.end();++iter )
- {
- ConstraintMem & cs = *iter;
- if( cs.active && (!cs.equality) && cs.rankIncreaser )
- {
- const double &l=cs.lagrangian=lagrangian(cs.range);
- if(l<HinactivationScore) { HinactivationScore=l; res=true; HinactivationRef=cs.constraintRow; }
- }
- sotDEBUG(45) << cs << std::endl;
+bool SolverHierarchicalInequalities::selecInactivationHierarchic(void) {
+ bool res = false;
+ double HinactivationScore = -THRESHOLD_ZERO;
+ for (ConstraintList::iterator iter = constraintH.begin();
+ iter != constraintH.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active && (!cs.equality) && cs.rankIncreaser) {
+ const double &l = cs.lagrangian = lagrangian(cs.range);
+ if (l < HinactivationScore) {
+ HinactivationScore = l;
+ res = true;
+ HinactivationRef = cs.constraintRow;
+ }
}
- if(res) {sotDEBUG(1) << "Inactivation H <" << constraintH[HinactivationRef].activeSide
- << HinactivationRef << ">" << constraintH[HinactivationRef] << std::endl;}
+ sotDEBUG(45) << cs << std::endl;
+ }
+ if (res) {
+ sotDEBUG(1) << "Inactivation H <"
+ << constraintH[HinactivationRef].activeSide << HinactivationRef
+ << ">" << constraintH[HinactivationRef] << std::endl;
+ }
return res;
}
/* The slack w = es-Js.u should be negative. */
-bool SolverHierarchicalInequalities::
-selecActivationSlack( void )
-{
- unsigned int row = 0; SactivationScore=THRESHOLD_ZERO;
- for( ConstraintList::const_iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter,++row )
- {
- // Slack should be negative
- if( (!iter->equality)&&(!iter->active) )
- {
- if( slackInf(row)>SactivationScore )
- { SactivationRef=row; SactivationScore=slackInf(row);
- SactivationSide=ConstraintMem::BOUND_INF; }
- if( slackSup(row)>SactivationScore )
- { SactivationRef=row; SactivationScore=slackSup(row);
- SactivationSide=ConstraintMem::BOUND_SUP; }
- }
+bool SolverHierarchicalInequalities::selecActivationSlack(void) {
+ unsigned int row = 0;
+ SactivationScore = THRESHOLD_ZERO;
+ for (ConstraintList::const_iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter, ++row) {
+ // Slack should be negative
+ if ((!iter->equality) && (!iter->active)) {
+ if (slackInf(row) > SactivationScore) {
+ SactivationRef = row;
+ SactivationScore = slackInf(row);
+ SactivationSide = ConstraintMem::BOUND_INF;
+ }
+ if (slackSup(row) > SactivationScore) {
+ SactivationRef = row;
+ SactivationScore = slackSup(row);
+ SactivationSide = ConstraintMem::BOUND_SUP;
+ }
}
- if( SactivationScore>THRESHOLD_ZERO )
- {sotDEBUG(1) << "Activation S <" << SactivationSide << SactivationRef << ">" << std::endl;}
- return SactivationScore>THRESHOLD_ZERO;
+ }
+ if (SactivationScore > THRESHOLD_ZERO) {
+ sotDEBUG(1) << "Activation S <" << SactivationSide << SactivationRef << ">"
+ << std::endl;
+ }
+ return SactivationScore > THRESHOLD_ZERO;
}
/* The slack should be negative. If strickly negative: unactivate. */
-bool SolverHierarchicalInequalities::
-selecInactivationSlack( void )
-{
- unsigned int row = 0; SinactivationScore=-THRESHOLD_ZERO;
- for( ConstraintList::const_iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter,++row )
- {
- if( (!iter->equality)&&(iter->active) )
- {
- if( (iter->activeSide&ConstraintMem::BOUND_INF)&&(slackInf(row)<SinactivationScore) )
- { SinactivationRef=row; SinactivationScore=slackInf(row);}
- if( (iter->activeSide&ConstraintMem::BOUND_SUP)&&(slackSup(row)<SinactivationScore) )
- { SinactivationRef=row; SinactivationScore=slackSup(row);}
- }
+bool SolverHierarchicalInequalities::selecInactivationSlack(void) {
+ unsigned int row = 0;
+ SinactivationScore = -THRESHOLD_ZERO;
+ for (ConstraintList::const_iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter, ++row) {
+ if ((!iter->equality) && (iter->active)) {
+ if ((iter->activeSide & ConstraintMem::BOUND_INF) &&
+ (slackInf(row) < SinactivationScore)) {
+ SinactivationRef = row;
+ SinactivationScore = slackInf(row);
+ }
+ if ((iter->activeSide & ConstraintMem::BOUND_SUP) &&
+ (slackSup(row) < SinactivationScore)) {
+ SinactivationRef = row;
+ SinactivationScore = slackSup(row);
+ }
}
- if( SinactivationScore<-THRESHOLD_ZERO )
- {sotDEBUG(1) << "Inactivation S <" <<constraintS[SinactivationRef].activeSide
- <<SinactivationRef << ">" << std::endl;}
- return (SinactivationScore<-THRESHOLD_ZERO); //TODO
+ }
+ if (SinactivationScore < -THRESHOLD_ZERO) {
+ sotDEBUG(1) << "Inactivation S <"
+ << constraintS[SinactivationRef].activeSide << SinactivationRef
+ << ">" << std::endl;
+ }
+ return (SinactivationScore < -THRESHOLD_ZERO); // TODO
}
/* ---------------------------------------------------------- */
-void SolverHierarchicalInequalities::
-pushBackSlackToHierarchy( void )
-{
- if( freeRank>0 )
- {
- /* Pe is the range of the actual column of QR in the original
- * matrix: QR[:,i] == Jse'[:,pe(i)]. */
- //sotDEBUG(15) << "pe = " << orderSe << std::endl;
- /* Ps is the range of the columns of Rs in the QR matrix:
- * R[:,i]=QR[:,ps(i)]. */
- sotDEBUG(15) << "ps = " << (MATLAB)orderS << std::endl;
- sotDEBUG(15) << "ranks = " << ranks << std::endl;
-
- /* Push back S constraints. */
- for( ConstraintList::iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter )
- {
- ConstraintMem& cs = *iter;
- if( cs.active )
- {
- const unsigned int nbcInJ = cs.constraintRow;
- if(! cs.equality )
- {
- if(cs.activeSide==ConstraintMem::BOUND_INF)
- cs.equality=(slackInf(nbcInJ)>THRESHOLD_ZERO);
- if(cs.activeSide==ConstraintMem::BOUND_SUP)
- cs.equality=(slackSup(nbcInJ)>THRESHOLD_ZERO);
- }
- }
- cs.constraintRow = constraintH.size();
- constraintH.push_back(cs);
- sotDEBUG(15) << "Add S cs: " <<cs << std::endl;
+void SolverHierarchicalInequalities::pushBackSlackToHierarchy(void) {
+ if (freeRank > 0) {
+ /* Pe is the range of the actual column of QR in the original
+ * matrix: QR[:,i] == Jse'[:,pe(i)]. */
+ // sotDEBUG(15) << "pe = " << orderSe << std::endl;
+ /* Ps is the range of the columns of Rs in the QR matrix:
+ * R[:,i]=QR[:,ps(i)]. */
+ sotDEBUG(15) << "ps = " << (MATLAB)orderS << std::endl;
+ sotDEBUG(15) << "ranks = " << ranks << std::endl;
+
+ /* Push back S constraints. */
+ for (ConstraintList::iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ if (cs.active) {
+ const unsigned int nbcInJ = cs.constraintRow;
+ if (!cs.equality) {
+ if (cs.activeSide == ConstraintMem::BOUND_INF)
+ cs.equality = (slackInf(nbcInJ) > THRESHOLD_ZERO);
+ if (cs.activeSide == ConstraintMem::BOUND_SUP)
+ cs.equality = (slackSup(nbcInJ) > THRESHOLD_ZERO);
}
+ }
+ cs.constraintRow = constraintH.size();
+ constraintH.push_back(cs);
+ sotDEBUG(15) << "Add S cs: " << cs << std::endl;
+ }
- /* Push back Rs. */
- sotRotationComposed Qlast;
- for( unsigned int i=0;i<ranks;++i )
- {
- typedef bub::matrix_column<bubMatrixQRWide> bubQJsCol;
- bubQJsCol QJk(QhJs,orderS(i)); Qlast.multiplyLeft(QJk);
- sotDEBUG(45) << "QhJs_" << i << " = " << (MATLAB)QJk << std::endl;
- bub::vector_range<bubQJsCol> Ftdown(QJk,freerange());
- double beta;
- double normSignFt
- = sotRotationSimpleHouseholder::householderExtraction(Ftdown,beta,THRESHOLD_ZERO);
- sotDEBUG(45) << "Ft_" << i << " = " << (MATLAB)Ftdown << std::endl;
- sotDEBUG(45) << "b_" << i << " = " << beta << std::endl;
- if(fabs(beta)>THRESHOLD_ZERO)
- { Qlast.pushBack(sotRotationSimpleHouseholder(Ftdown,beta)); }
-
- bub::matrix_column<bubMatrix> Rhk(Rh,rankh);
- bub::project(Rhk,rangeh()).assign( bub::project(QJk,rangeh()) );
- bub::project(Rhk,freerange()).assign(bub::zero_vector<double>(freeRank));
- Rh(rankh,rankh) = normSignFt;
- sotDEBUG(45) << "Rh_" << i << " = " << (MATLAB)Rhk << std::endl;
-
- const unsigned int colInH = constraintSactive[orderS(i)]->constraintRow;
- constraintH[colInH].range=rankh;
- sotDEBUG(15) << "Constraint S " << i << " (rangeS=" << orderS(i)
- << ") set in H " << rankh << ": " << constraintH[colInH] << std::endl;
- rankh++; freeRank--;
- }
- Qh.pushBack(Qlast);
+ /* Push back Rs. */
+ sotRotationComposed Qlast;
+ for (unsigned int i = 0; i < ranks; ++i) {
+ typedef bub::matrix_column<bubMatrixQRWide> bubQJsCol;
+ bubQJsCol QJk(QhJs, orderS(i));
+ Qlast.multiplyLeft(QJk);
+ sotDEBUG(45) << "QhJs_" << i << " = " << (MATLAB)QJk << std::endl;
+ bub::vector_range<bubQJsCol> Ftdown(QJk, freerange());
+ double beta;
+ double normSignFt = sotRotationSimpleHouseholder::householderExtraction(
+ Ftdown, beta, THRESHOLD_ZERO);
+ sotDEBUG(45) << "Ft_" << i << " = " << (MATLAB)Ftdown << std::endl;
+ sotDEBUG(45) << "b_" << i << " = " << beta << std::endl;
+ if (fabs(beta) > THRESHOLD_ZERO) {
+ Qlast.pushBack(sotRotationSimpleHouseholder(Ftdown, beta));
+ }
+ bub::matrix_column<bubMatrix> Rhk(Rh, rankh);
+ bub::project(Rhk, rangeh()).assign(bub::project(QJk, rangeh()));
+ bub::project(Rhk, freerange()).assign(bub::zero_vector<double>(freeRank));
+ Rh(rankh, rankh) = normSignFt;
+ sotDEBUG(45) << "Rh_" << i << " = " << (MATLAB)Rhk << std::endl;
+
+ const unsigned int colInH = constraintSactive[orderS(i)]->constraintRow;
+ constraintH[colInH].range = rankh;
+ sotDEBUG(15) << "Constraint S " << i << " (rangeS=" << orderS(i)
+ << ") set in H " << rankh << ": " << constraintH[colInH]
+ << std::endl;
+ rankh++;
+ freeRank--;
}
- else
- {
- for( ConstraintList::iterator iter=constraintS.begin();
- iter!=constraintS.end();++iter )
- {
- ConstraintMem & cs = *iter;
- cs.rankIncreaser = false;
- cs.constraintRow = constraintH.size();
- constraintH.push_back(cs);
- sotDEBUG(15) << "Add rank-void cs: " << constraintH.back() << std::endl;
- }
+ Qh.pushBack(Qlast);
+
+ } else {
+ for (ConstraintList::iterator iter = constraintS.begin();
+ iter != constraintS.end(); ++iter) {
+ ConstraintMem &cs = *iter;
+ cs.rankIncreaser = false;
+ cs.constraintRow = constraintH.size();
+ constraintH.push_back(cs);
+ sotDEBUG(15) << "Add rank-void cs: " << constraintH.back() << std::endl;
}
+ }
- if( rankh+freeRank!=nJ )
- {
- std::cerr << "Error: ( rankh+freeRank!=nJ ). " << std::endl;
- throw "Error: ( rankh+freeRank!=nJ ). ";
- }
+ if (rankh + freeRank != nJ) {
+ std::cerr << "Error: ( rankh+freeRank!=nJ ). " << std::endl;
+ throw "Error: ( rankh+freeRank!=nJ ). ";
+ }
sotDEBUG(15) << "Qh = " << Qh << std::endl;
sotDEBUG(15) << "Rh = " << (MATLAB)accessRhConst() << std::endl;
}
-
double SolverHierarchicalInequalities::THRESHOLD_ZERO = 1e-6;
diff --git a/src/sot/sot-command.h b/src/sot/sot-command.h
index ee9d6fe7..b1f456b5 100644
--- a/src/sot/sot-command.h
+++ b/src/sot/sot-command.h
@@ -7,227 +7,194 @@
*/
#ifndef SOT_COMMAND_H
- #define SOT_COMMAND_H
-
- #include <boost/assign/list_of.hpp>
-
- #include <dynamic-graph/command.h>
- #include <dynamic-graph/command-setter.h>
- #include <dynamic-graph/command-getter.h>
-
-namespace dynamicgraph { namespace sot {
- namespace command {
- namespace classSot {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
-
- // Command AddConstraint
- class AddConstraint : public Command
- {
- public:
- virtual ~AddConstraint() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- AddConstraint(Sot& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::STRING), docstring)
- {
- }
- virtual Value doExecute()
- {
- Sot& sot = static_cast<Sot&>(owner());
- std::vector<Value> values = getParameterValues();
- std::string constraintName = values[0].value();
-
- Constraint& constraint =
- dynamic_cast<Constraint&>(PoolStorage::getInstance()->
- getTask(constraintName));
- sot.addConstraint(constraint);
- sot.constraintSOUT.setReady();
- // return void
- return Value();
- }
- }; // class AddConstraint
-
- // Command Push
- class Push : public Command
- {
- public:
- virtual ~Push() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Push(Sot& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::STRING), docstring)
- {
- }
- virtual Value doExecute()
- {
- Sot& sot = static_cast<Sot&>(owner());
- std::vector<Value> values = getParameterValues();
- std::string taskName = values[0].value();
-
- TaskAbstract& task = PoolStorage::getInstance()->getTask(taskName);
- sot.push(task);
- // return void
- return Value();
- }
- }; // class Push
-
- // Command Remove
- class Remove : public Command
- {
- public:
- virtual ~Remove() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Remove(Sot& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::STRING), docstring)
- {
- }
- virtual Value doExecute()
- {
- Sot& sot = static_cast<Sot&>(owner());
- std::vector<Value> values = getParameterValues();
- std::string taskName = values[0].value();
-
- TaskAbstract& task = PoolStorage::getInstance()->getTask(taskName);
- sot.remove(task);
- // return void
- return Value();
- }
- }; // class Remove
-
- // Command Up
- class Up : public Command
- {
- public:
- virtual ~Up() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Up(Sot& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::STRING), docstring)
- {
- }
- virtual Value doExecute()
- {
- Sot& sot = static_cast<Sot&>(owner());
- std::vector<Value> values = getParameterValues();
- std::string taskName = values[0].value();
-
- TaskAbstract& task = PoolStorage::getInstance()->getTask(taskName);
- sot.up(task);
- // return void
- return Value();
- }
- }; // class Remove
-
- // Command Down
- class Down : public Command
- {
- public:
- virtual ~Down() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Down(Sot& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::STRING), docstring)
- {
- }
- virtual Value doExecute()
- {
- Sot& sot = static_cast<Sot&>(owner());
- std::vector<Value> values = getParameterValues();
- std::string taskName = values[0].value();
-
- TaskAbstract& task = PoolStorage::getInstance()->getTask(taskName);
- sot.down(task);
- // return void
- return Value();
- }
- }; // class Down
-
- // Command Display
- class Display : public Command
- {
- public:
- virtual ~Display() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- Display(Sot& entity, const std::string& docstring) :
- Command(entity, std::vector<Value::Type> (), docstring)
- {
- }
- virtual Value doExecute()
- {
- std::stringstream returnString;
- Sot& sot = static_cast<Sot&>(owner());
- sot.display (returnString);
-
- // return the stack
- return Value(returnString.str());
- }
- }; // class Display
-
- // Command List
- class List : public Command
- {
- public:
- virtual ~List() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- List(Sot& entity, const std::string& docstring) :
- Command(entity, std::vector<Value::Type> (), docstring)
- {
- }
- virtual Value doExecute()
- {
- Sot& sot = static_cast<Sot&>(owner());
- typedef Sot::StackType StackType;
- const StackType& stack = sot.tasks();
-
- std::stringstream returnString;
- returnString << "( ";
- for (StackType::const_iterator it = stack.begin();
- it != stack.end(); ++it)
- {
- returnString << '"' << (*it)->getName() << "\", ";
- }
- returnString << ")";
-
- // return the stack
- return Value(returnString.str());
- }
- }; // class List
-
- // Command Clear
- class Clear : public Command
- {
- public:
- virtual ~Clear() {}
- /// Clear the stack
- /// \param docstring documentation of the command
- Clear(Sot& entity, const std::string& docstring) :
- Command(entity, std::vector<Value::Type> (), docstring)
- {
- }
- virtual Value doExecute()
- {
- std::stringstream returnString;
- Sot& sot = static_cast<Sot&>(owner());
- sot.clear();
- // return the stack
- return Value();
- }
- }; // class Clear
-
-
- } // namespace classSot
- } // namespace command
-} /* namespace sot */} /* namespace dynamicgraph */
-
-#endif //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>
+
+namespace dynamicgraph {
+namespace sot {
+namespace command {
+namespace classSot {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+// Command AddConstraint
+class AddConstraint : public Command {
+public:
+ virtual ~AddConstraint() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ AddConstraint(Sot &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+ virtual Value doExecute() {
+ Sot &sot = static_cast<Sot &>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string constraintName = values[0].value();
+
+ Constraint &constraint = dynamic_cast<Constraint &>(
+ PoolStorage::getInstance()->getTask(constraintName));
+ sot.addConstraint(constraint);
+ sot.constraintSOUT.setReady();
+ // return void
+ return Value();
+ }
+}; // class AddConstraint
+
+// Command Push
+class Push : public Command {
+public:
+ virtual ~Push() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Push(Sot &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+ virtual Value doExecute() {
+ Sot &sot = static_cast<Sot &>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string taskName = values[0].value();
+
+ TaskAbstract &task = PoolStorage::getInstance()->getTask(taskName);
+ sot.push(task);
+ // return void
+ return Value();
+ }
+}; // class Push
+
+// Command Remove
+class Remove : public Command {
+public:
+ virtual ~Remove() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Remove(Sot &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+ virtual Value doExecute() {
+ Sot &sot = static_cast<Sot &>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string taskName = values[0].value();
+
+ TaskAbstract &task = PoolStorage::getInstance()->getTask(taskName);
+ sot.remove(task);
+ // return void
+ return Value();
+ }
+}; // class Remove
+
+// Command Up
+class Up : public Command {
+public:
+ virtual ~Up() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Up(Sot &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+ virtual Value doExecute() {
+ Sot &sot = static_cast<Sot &>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string taskName = values[0].value();
+
+ TaskAbstract &task = PoolStorage::getInstance()->getTask(taskName);
+ sot.up(task);
+ // return void
+ return Value();
+ }
+}; // class Remove
+
+// Command Down
+class Down : public Command {
+public:
+ virtual ~Down() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Down(Sot &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+ virtual Value doExecute() {
+ Sot &sot = static_cast<Sot &>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string taskName = values[0].value();
+
+ TaskAbstract &task = PoolStorage::getInstance()->getTask(taskName);
+ sot.down(task);
+ // return void
+ return Value();
+ }
+}; // class Down
+
+// Command Display
+class Display : public Command {
+public:
+ virtual ~Display() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Display(Sot &entity, const std::string &docstring)
+ : Command(entity, std::vector<Value::Type>(), docstring) {}
+ virtual Value doExecute() {
+ std::stringstream returnString;
+ Sot &sot = static_cast<Sot &>(owner());
+ sot.display(returnString);
+
+ // return the stack
+ return Value(returnString.str());
+ }
+}; // class Display
+
+// Command List
+class List : public Command {
+public:
+ virtual ~List() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ List(Sot &entity, const std::string &docstring)
+ : Command(entity, std::vector<Value::Type>(), docstring) {}
+ virtual Value doExecute() {
+ Sot &sot = static_cast<Sot &>(owner());
+ typedef Sot::StackType StackType;
+ const StackType &stack = sot.tasks();
+
+ std::stringstream returnString;
+ returnString << "( ";
+ for (StackType::const_iterator it = stack.begin(); it != stack.end();
+ ++it) {
+ returnString << '"' << (*it)->getName() << "\", ";
+ }
+ returnString << ")";
+
+ // return the stack
+ return Value(returnString.str());
+ }
+}; // class List
+
+// Command Clear
+class Clear : public Command {
+public:
+ virtual ~Clear() {}
+ /// Clear the stack
+ /// \param docstring documentation of the command
+ Clear(Sot &entity, const std::string &docstring)
+ : Command(entity, std::vector<Value::Type>(), docstring) {}
+ virtual Value doExecute() {
+ std::stringstream returnString;
+ Sot &sot = static_cast<Sot &>(owner());
+ sot.clear();
+ // return the stack
+ return Value();
+ }
+}; // class Clear
+
+} // namespace classSot
+} // namespace command
+} /* namespace sot */
+} /* namespace dynamicgraph */
+
+#endif // SOT_COMMAND_H
diff --git a/src/sot/sot-h.cpp b/src/sot/sot-h.cpp
index e05db0a2..0feabfa7 100644
--- a/src/sot/sot-h.cpp
+++ b/src/sot/sot-h.cpp
@@ -12,20 +12,20 @@
//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <sot/core/debug.hh>
-#include <sot/core/sot-h.hh>
+#include <sot/core/factory.hh>
#include <sot/core/pool.hh>
-#include <sot/core/task.hh>
+#include <sot/core/sot-h.hh>
#include <sot/core/task-unilateral.hh>
-#include <sot/core/factory.hh>
+#include <sot/core/task.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
#ifdef VP_DEBUG
-class sotSOTH__INIT
-{
-public:sotSOTH__INIT( void ) { sot::DebugTrace::openFile(); }
+class sotSOTH__INIT {
+public:
+ sotSOTH__INIT(void) { sot::DebugTrace::openFile(); }
};
sotSOTH__INIT sotSOTH_initiator;
@@ -35,262 +35,260 @@ sotSOTH__INIT sotSOTH_initiator;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotH,"SOTH");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotH, "SOTH");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-SotH::
-SotH( const std::string& name )
- :Sot(name),Qh(nbJoints),Rh(nbJoints,nbJoints),constraintH()
- ,solverNorm(nbJoints,Qh,Rh,constraintH),solverPrec(NULL)
- ,fillMemorySignal(false)
-{
-}
+SotH::SotH(const std::string &name)
+ : Sot(name), Qh(nbJoints), Rh(nbJoints, nbJoints), constraintH(),
+ solverNorm(nbJoints, Qh, Rh, constraintH), solverPrec(NULL),
+ fillMemorySignal(false) {}
+SotH::~SotH(void) {}
-SotH::
-~SotH( void )
-{
-
-
-
-}
-
-
-void SotH::
-defineNbDof( const unsigned int& nbDof )
-{
+void SotH::defineNbDof(const unsigned int &nbDof) {
sotDEBUGIN(15);
Sot::defineNbDof(nbDof);
- for(StackType::iterator iter=stack.begin();stack.end()!=iter;++iter )
- {
- MemoryTaskSOTH * mem
- = dynamic_cast<MemoryTaskSOTH *>( (*iter)->memoryInternal );
- if(NULL!=mem) mem->solver.setNbDof(nbDof);
- }
- solverNorm.setNbDof(nbDof-(ffJointIdLast-ffJointIdFirst));
+ for (StackType::iterator iter = stack.begin(); stack.end() != iter; ++iter) {
+ MemoryTaskSOTH *mem =
+ dynamic_cast<MemoryTaskSOTH *>((*iter)->memoryInternal);
+ if (NULL != mem)
+ mem->solver.setNbDof(nbDof);
+ }
+ solverNorm.setNbDof(nbDof - (ffJointIdLast - ffJointIdFirst));
sotDEBUGOUT(15);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void buildTaskVectors( const VectorMultiBound& err,
- const dynamicgraph::Matrix & JK,
- bubVector & ee,bubVector & eiinf,bubVector & eisup,
- ConstraintMem::BoundSideVector& bounds,
- bubMatrix & Je,bubMatrix & Ji )
-{
+void buildTaskVectors(const VectorMultiBound &err,
+ const dynamicgraph::Matrix &JK, bubVector &ee,
+ bubVector &eiinf, bubVector &eisup,
+ ConstraintMem::BoundSideVector &bounds, bubMatrix &Je,
+ bubMatrix &Ji) {
const unsigned int nJ = JK.cols();
- sotDEBUG(25) << "/* Compute the task sizes. */"<< std::endl;
- unsigned int sizei=0,sizee=0;
- for( VectorMultiBound::const_iterator iter=err.begin();
- err.end()!=iter;++iter )
- { if( iter->getMode()==MultiBound::MODE_SINGLE )
- sizee++; else sizei++; }
+ sotDEBUG(25) << "/* Compute the task sizes. */" << std::endl;
+ unsigned int sizei = 0, sizee = 0;
+ for (VectorMultiBound::const_iterator iter = err.begin(); err.end() != iter;
+ ++iter) {
+ if (iter->getMode() == MultiBound::MODE_SINGLE)
+ sizee++;
+ else
+ sizei++;
+ }
- Ji.resize(sizei,nJ); eiinf.resize(sizei),eisup.resize(sizei);
+ Ji.resize(sizei, nJ);
+ eiinf.resize(sizei), eisup.resize(sizei);
bounds.resize(sizei);
- Je.resize(sizee,nJ); ee.resize(sizee);
+ Je.resize(sizee, nJ);
+ ee.resize(sizee);
sotDEBUG(25) << "Size e=" << sizee << " i=" << sizei << std::endl;
- sotDEBUG(25) << "/* Build the task vectors. */"<< std::endl;
- for( int i=err.size()-1;i>=0;--i )
- {
- sotDEBUG(25) << "i = "<<i<< std::endl;
- switch( err[i].getMode() )
- {
- case MultiBound::MODE_SINGLE:
- {
- ee(--sizee) = err[i].getSingleBound();
- for( unsigned int j=0;j<nJ;++j )
- { Je(sizee,j) = JK(i,j); }
- break;
- }
- case MultiBound::MODE_DOUBLE:
- {
- --sizei;
- bounds[sizei] = ConstraintMem::BOUND_VOID;
- if( err[i].getDoubleBoundSetup( MultiBound::BOUND_INF ) )
- {
- eiinf(sizei) = err[i].getDoubleBound( MultiBound::BOUND_INF );
- bounds[sizei] = ConstraintMem::BOUND_INF;
- }
- if( err[i].getDoubleBoundSetup( MultiBound::BOUND_SUP ) )
- {
- eisup(sizei) = err[i].getDoubleBound( MultiBound::BOUND_SUP );
- if( bounds[sizei]==ConstraintMem::BOUND_INF )
- bounds[sizei]= ConstraintMem::BOUND_BOTH;
- else bounds[sizei]= ConstraintMem::BOUND_SUP;
- }
- for( unsigned int j=0;j<nJ;++j )
- { Ji(sizei,j) = JK(i,j); }
- break;
- }
- }
+ sotDEBUG(25) << "/* Build the task vectors. */" << std::endl;
+ for (int i = err.size() - 1; i >= 0; --i) {
+ sotDEBUG(25) << "i = " << i << std::endl;
+ switch (err[i].getMode()) {
+ case MultiBound::MODE_SINGLE: {
+ ee(--sizee) = err[i].getSingleBound();
+ for (unsigned int j = 0; j < nJ; ++j) {
+ Je(sizee, j) = JK(i, j);
+ }
+ break;
+ }
+ case MultiBound::MODE_DOUBLE: {
+ --sizei;
+ bounds[sizei] = ConstraintMem::BOUND_VOID;
+ if (err[i].getDoubleBoundSetup(MultiBound::BOUND_INF)) {
+ eiinf(sizei) = err[i].getDoubleBound(MultiBound::BOUND_INF);
+ bounds[sizei] = ConstraintMem::BOUND_INF;
+ }
+ if (err[i].getDoubleBoundSetup(MultiBound::BOUND_SUP)) {
+ eisup(sizei) = err[i].getDoubleBound(MultiBound::BOUND_SUP);
+ if (bounds[sizei] == ConstraintMem::BOUND_INF)
+ bounds[sizei] = ConstraintMem::BOUND_BOTH;
+ else
+ bounds[sizei] = ConstraintMem::BOUND_SUP;
+ }
+ for (unsigned int j = 0; j < nJ; ++j) {
+ Ji(sizei, j) = JK(i, j);
+ }
+ break;
+ }
}
- sotDEBUG(25) << "ee = "<<(MATLAB)ee<< std::endl;
- sotDEBUG(25) << "eii = "<<(MATLAB)eiinf<< std::endl;
- sotDEBUG(25) << "eis = "<<(MATLAB)eisup<< std::endl;
+ }
+ sotDEBUG(25) << "ee = " << (MATLAB)ee << std::endl;
+ sotDEBUG(25) << "eii = " << (MATLAB)eiinf << std::endl;
+ sotDEBUG(25) << "eis = " << (MATLAB)eisup << std::endl;
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#ifdef WITH_CHRONO
- #ifndef WIN32
- #include <sys/time.h>
- #else /*WIN32*/
- #include <sot/core/utils-windows.hh>
- #endif /*WIN32*/
+#ifdef WITH_CHRONO
+#ifndef WIN32
+#include <sys/time.h>
+#else /*WIN32*/
+#include <sot/core/utils-windows.hh>
+#endif /*WIN32*/
#endif /*WITH_CHRONO*/
#ifdef WITH_CHRONO
-# define SOT_DEFINE_CHRONO \
- struct timeval t0,t1; \
- double dtsolver; \
+#define SOT_DEFINE_CHRONO \
+ struct timeval t0, t1; \
+ double dtsolver; \
std::ofstream foutdt("/tmp/dtsoth.dat")
-# define SOT_INIT_CHRONO \
- gettimeofday(&t0,NULL); foutdt<<std::endl<<iterTime<<" "
-# define SOT_CHRONO \
- gettimeofday(&t1,NULL); \
- dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ; \
- foutdt << dtsolver <<" "; \
- gettimeofday(&t0,NULL)
+#define SOT_INIT_CHRONO \
+ gettimeofday(&t0, NULL); \
+ foutdt << std::endl << iterTime << " "
+#define SOT_CHRONO \
+ gettimeofday(&t1, NULL); \
+ dtsolver = (t1.tv_sec - t0.tv_sec) * 1000. + \
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.; \
+ foutdt << dtsolver << " "; \
+ gettimeofday(&t0, NULL)
#else // ifdef WITH_CHRONO
-# define SOT_DEFINE_CHRONO
-# define SOT_INIT_CHRONO
-# define SOT_CHRONO()
+#define SOT_DEFINE_CHRONO
+#define SOT_INIT_CHRONO
+#define SOT_CHRONO()
#endif // ifdef WITH_CHRONO
-
SOT_DEFINE_CHRONO;
-dynamicgraph::Vector& SotH::
-computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
-{
+dynamicgraph::Vector &SotH::computeControlLaw(dynamicgraph::Vector &control,
+ const int &iterTime) {
sotDEBUGIN(15);
SOT_INIT_CHRONO;
- SolverHierarchicalInequalities::THRESHOLD_ZERO = inversionThresholdSIN(iterTime);
+ SolverHierarchicalInequalities::THRESHOLD_ZERO =
+ inversionThresholdSIN(iterTime);
const dynamicgraph::Matrix &K = constraintSOUT(iterTime);
const unsigned int nJ = K.cols();
- try
- {
- control = q0SIN( iterTime );
- sotDEBUG(15) << "initial velocity q0 = " << control << endl;
- if( nJ!=control.size() ) { control.resize( nJ ); control.fill(.0); }
+ try {
+ control = q0SIN(iterTime);
+ sotDEBUG(15) << "initial velocity q0 = " << control << endl;
+ if (nJ != control.size()) {
+ control.resize(nJ);
+ control.fill(.0);
}
- catch (...)
- {
- if( nJ!=control.size() ) { control.resize( nJ ); }
- control.setZero();
- sotDEBUG(25) << "No initial velocity." <<endl;
+ } catch (...) {
+ if (nJ != control.size()) {
+ control.resize(nJ);
}
+ control.setZero();
+ sotDEBUG(25) << "No initial velocity." << endl;
+ }
SOT_CHRONO;
/* --- Affect solvers --- */
-// if( solvers.size()+1!=stack.size() )
-// {
-// solvers.resize(stack.size()+1,NULL); // TODO: resize affect to default value?
-// for( SolversList::iterator iter = solvers.begin();solvers.end()!=iter;++iter )
-// {
-// if(*iter==NULL)
-// {
-// (*iter)=new SolverHierarchicalInequalities(nJ,Qh,Rh,constraintH);
-// }
-// /* TODO: free memory */
-// }
-// }
-
- if(Qh.getSize()!=nJ) { Qh.clear(nJ); }
- if((Rh.size1()!=nJ)||(Rh.size2()!=nJ)) Rh.resize(nJ,nJ,false);
- Rh*=0;
+ // if( solvers.size()+1!=stack.size() )
+ // {
+ // solvers.resize(stack.size()+1,NULL); // TODO: resize affect to
+ // default value? for( SolversList::iterator iter =
+ // solvers.begin();solvers.end()!=iter;++iter )
+ // {
+ // if(*iter==NULL)
+ // {
+ // (*iter)=new
+ // SolverHierarchicalInequalities(nJ,Qh,Rh,constraintH);
+ // }
+ // /* TODO: free memory */
+ // }
+ // }
+
+ if (Qh.getSize() != nJ) {
+ Qh.clear(nJ);
+ }
+ if ((Rh.size1() != nJ) || (Rh.size2() != nJ))
+ Rh.resize(nJ, nJ, false);
+ Rh *= 0;
constraintH.clear();
- solverPrec=NULL;
+ solverPrec = NULL;
SOT_CHRONO;
- sotDEBUGF(5, " --- Time %d -------------------", iterTime );
+ sotDEBUGF(5, " --- Time %d -------------------", iterTime);
unsigned int iterTask = 0;
- for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter,++iterTask )
- {
- sotDEBUGF(5,"Rank %d.",iterTask);
- TaskAbstract & task = **iter;
- sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
- const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
- const VectorMultiBound &err = task.taskSOUT(iterTime);
- unsigned int ntask=err.size();
-
- sotDEBUG(25) << "/* Init memory. */" << std::endl;
- MemoryTaskSOTH * mem = dynamic_cast<MemoryTaskSOTH *>( task.memoryInternal );
- if( (NULL==mem)||(mem->referenceKey!=this) )
- {
- if( NULL!=task.memoryInternal ) delete task.memoryInternal;
- mem = new MemoryTaskSOTH(task.getName()+"_memSOTH",
- this,nJ,Qh,Rh,constraintH);
- task.memoryInternal = mem;
- }
+ for (StackType::iterator iter = stack.begin(); iter != stack.end();
+ ++iter, ++iterTask) {
+ sotDEBUGF(5, "Rank %d.", iterTask);
+ TaskAbstract &task = **iter;
+ sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
+ const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
+ const VectorMultiBound &err = task.taskSOUT(iterTime);
+ unsigned int ntask = err.size();
+
+ sotDEBUG(25) << "/* Init memory. */" << std::endl;
+ MemoryTaskSOTH *mem = dynamic_cast<MemoryTaskSOTH *>(task.memoryInternal);
+ if ((NULL == mem) || (mem->referenceKey != this)) {
+ if (NULL != task.memoryInternal)
+ delete task.memoryInternal;
+ mem = new MemoryTaskSOTH(task.getName() + "_memSOTH", this, nJ, Qh, Rh,
+ constraintH);
+ task.memoryInternal = mem;
+ }
- sotDEBUG(25) << "/* --- Resize J --- */" << std::endl;
- mem->JK.resize( ntask,nJ );
- mem->Jff.resize( ntask,Jac.cols()-nJ );
- mem->Jact.resize( ntask,nJ );
-
- sotDEBUG(25) << "/* --- COMPUTE JK --- */" << std::endl;
- Sot::computeJacobianConstrained( Jac,K,mem->JK,mem->Jff,mem->Jact );
- if( fillMemorySignal )
- {
- mem->jacobianConstrainedSINOUT = mem->JK;
- mem->jacobianConstrainedSINOUT.setTime( iterTime );
- }
+ sotDEBUG(25) << "/* --- Resize J --- */" << std::endl;
+ mem->JK.resize(ntask, nJ);
+ mem->Jff.resize(ntask, Jac.cols() - nJ);
+ mem->Jact.resize(ntask, nJ);
- sotDEBUG(25) << "/* Set initial conditions. */" << std::endl;
- SolverHierarchicalInequalities & solver = mem->solver;
- if( NULL==solverPrec ) solver.setInitialConditionVoid();
- else
- { solver.setInitialCondition( solverPrec->u0,solverPrec->rankh ); }
- solverPrec = &solver;
- SOT_CHRONO;
+ sotDEBUG(25) << "/* --- COMPUTE JK --- */" << std::endl;
+ Sot::computeJacobianConstrained(Jac, K, mem->JK, mem->Jff, mem->Jact);
+ if (fillMemorySignal) {
+ mem->jacobianConstrainedSINOUT = mem->JK;
+ mem->jacobianConstrainedSINOUT.setTime(iterTime);
+ }
+
+ sotDEBUG(25) << "/* Set initial conditions. */" << std::endl;
+ SolverHierarchicalInequalities &solver = mem->solver;
+ if (NULL == solverPrec)
+ solver.setInitialConditionVoid();
+ else {
+ solver.setInitialCondition(solverPrec->u0, solverPrec->rankh);
+ }
+ solverPrec = &solver;
+ SOT_CHRONO;
#ifdef VP_DEBUG
- solver.printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
+ solver.printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
#endif
- sotDEBUG(25) << "/* Record warm start. */" << std::endl;
- solver.recordInitialConditions();
-
- sotDEBUG(25) << "/* Effective warm start. */ " << std::endl;
- solver.warmStart();
- SOT_CHRONO;
-
- sotDEBUG(25) << "/* Build the task vectors. */"<< std::endl;
- bubMatrix Ji; bubVector eiinf,eisup;
- ConstraintMem::BoundSideVector bounds;
- bubMatrix Je; bubVector ee;
- buildTaskVectors(err,mem->JK,ee,eiinf,eisup,bounds,Je,Ji);
- sotDEBUG(15) << "ee" << iterTask << " = " << ee << std::endl;
- sotDEBUG(15) << "eiinf" << iterTask << " = " << eiinf << std::endl;
- sotDEBUG(15) << "eisup" << iterTask << " = " << eisup << std::endl;
- SOT_CHRONO;
-
- sotDEBUG(25) << "/* Solve level=" <<iterTask<<". */"<< std::endl;
- solver.solve( Je,ee,Ji,eiinf,eisup,bounds,solver.getSlackActiveSet() );
- sotDEBUG(1) << "u" << iterTask << " = " << (MATLAB)solver.u0 << endl;
- SOT_CHRONO;
-
- solver.computeDifferentialCondition();
+ sotDEBUG(25) << "/* Record warm start. */" << std::endl;
+ solver.recordInitialConditions();
+
+ sotDEBUG(25) << "/* Effective warm start. */ " << std::endl;
+ solver.warmStart();
+ SOT_CHRONO;
+
+ sotDEBUG(25) << "/* Build the task vectors. */" << std::endl;
+ bubMatrix Ji;
+ bubVector eiinf, eisup;
+ ConstraintMem::BoundSideVector bounds;
+ bubMatrix Je;
+ bubVector ee;
+ buildTaskVectors(err, mem->JK, ee, eiinf, eisup, bounds, Je, Ji);
+ sotDEBUG(15) << "ee" << iterTask << " = " << ee << std::endl;
+ sotDEBUG(15) << "eiinf" << iterTask << " = " << eiinf << std::endl;
+ sotDEBUG(15) << "eisup" << iterTask << " = " << eisup << std::endl;
+ SOT_CHRONO;
+
+ sotDEBUG(25) << "/* Solve level=" << iterTask << ". */" << std::endl;
+ solver.solve(Je, ee, Ji, eiinf, eisup, bounds, solver.getSlackActiveSet());
+ sotDEBUG(1) << "u" << iterTask << " = " << (MATLAB)solver.u0 << endl;
+ SOT_CHRONO;
+
+ solver.computeDifferentialCondition();
#ifdef VP_DEBUG
- solver.printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
+ solver.printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
#endif
- SOT_CHRONO;
- }
+ SOT_CHRONO;
+ }
/* --- Minimize norm --- */
{
@@ -300,7 +298,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
sotDEBUG(25) << solverNorm.u0 << std::endl;
sotDEBUG(25) << solverNorm.nJ << std::endl;
sotDEBUG(25) << nJ << std::endl;
- solverNorm.setInitialCondition( solverPrec->u0,solverPrec->rankh );
+ solverNorm.setInitialCondition(solverPrec->u0, solverPrec->rankh);
SOT_CHRONO;
sotDEBUG(25) << "/* Record warm start. */" << std::endl;
@@ -309,11 +307,15 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
solverNorm.warmStart();
SOT_CHRONO;
- sotDEBUG(25) << "/* Solve. */"<< std::endl;
- bubMatrix Idnj( bub::identity_matrix<double>(nJ,nJ));
- bubVector ee(nJ); ee.assign( bub::zero_vector<double>(nJ));
- bubMatrix Ji(0,0); bubVector ei(0); ConstraintMem::BoundSideVector bounds(0);
- solverNorm.solve( Idnj,ee,Ji,ei,ei,bounds,solverNorm.getSlackActiveSet() );
+ sotDEBUG(25) << "/* Solve. */" << std::endl;
+ bubMatrix Idnj(bub::identity_matrix<double>(nJ, nJ));
+ bubVector ee(nJ);
+ ee.assign(bub::zero_vector<double>(nJ));
+ bubMatrix Ji(0, 0);
+ bubVector ei(0);
+ ConstraintMem::BoundSideVector bounds(0);
+ solverNorm.solve(Idnj, ee, Ji, ei, ei, bounds,
+ solverNorm.getSlackActiveSet());
sotDEBUG(1) << "utop = " << (MATLAB)solverNorm.u0 << endl;
SOT_CHRONO;
@@ -326,37 +328,37 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
/* Passing the result to the control. */
control.resize(nJ);
- control.accessToMotherLib().assign( solverNorm.u0 );
-
- if( fillMemorySignal )
- for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter,++iterTask )
- {
- TaskAbstract & task = **iter;
- MemoryTaskSOTH * mem = dynamic_cast<MemoryTaskSOTH *>( task.memoryInternal );
- if( mem == NULL ) continue;
- VectorMultiBound taskVector = task.taskSOUT(iterTime);
- const dynamicgraph::Matrix JK = mem->jacobianConstrainedSINOUT.accessCopy();
- dynamicgraph::Vector JKu(taskVector.size()); JKu = JK*control;
- dynamicgraph::Vector diff(taskVector.size());
-
- for( unsigned int i=0;i<taskVector.size();++i )
- {
- const MultiBound & Xi = taskVector[i];
- switch( Xi.getMode() )
- {
- case MultiBound::MODE_SINGLE:
- diff(i) = Xi.getSingleBound()-JKu(i);
- break;
- case MultiBound::MODE_DOUBLE:
- diff(i) = std::min( JKu(i)-Xi.getDoubleBound(MultiBound::BOUND_INF ),
- Xi.getDoubleBound(MultiBound::BOUND_SUP)-JKu(i) );
- break;
- }
- }
- mem->diffErrorSINOUT = diff;
- mem->diffErrorSINOUT.setTime( iterTime );
-
+ control.accessToMotherLib().assign(solverNorm.u0);
+
+ if (fillMemorySignal)
+ for (StackType::iterator iter = stack.begin(); iter != stack.end();
+ ++iter, ++iterTask) {
+ TaskAbstract &task = **iter;
+ MemoryTaskSOTH *mem = dynamic_cast<MemoryTaskSOTH *>(task.memoryInternal);
+ if (mem == NULL)
+ continue;
+ VectorMultiBound taskVector = task.taskSOUT(iterTime);
+ const dynamicgraph::Matrix JK =
+ mem->jacobianConstrainedSINOUT.accessCopy();
+ dynamicgraph::Vector JKu(taskVector.size());
+ JKu = JK * control;
+ dynamicgraph::Vector diff(taskVector.size());
+
+ for (unsigned int i = 0; i < taskVector.size(); ++i) {
+ const MultiBound &Xi = taskVector[i];
+ switch (Xi.getMode()) {
+ case MultiBound::MODE_SINGLE:
+ diff(i) = Xi.getSingleBound() - JKu(i);
+ break;
+ case MultiBound::MODE_DOUBLE:
+ diff(i) = std::min(JKu(i) - Xi.getDoubleBound(MultiBound::BOUND_INF),
+ Xi.getDoubleBound(MultiBound::BOUND_SUP) - JKu(i));
+ break;
+ }
}
+ mem->diffErrorSINOUT = diff;
+ mem->diffErrorSINOUT.setTime(iterTime);
+ }
sotDEBUGOUT(15);
return control;
@@ -366,21 +368,17 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
/* --- MEMORY ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
const std::string SotH::MemoryTaskSOTH::CLASS_NAME = "MemoryTaskSOTH";
-void SotH::MemoryTaskSOTH::
-display( std::ostream& /*os*/ ) const {} //TODO
-
-
-SotH::MemoryTaskSOTH::
-MemoryTaskSOTH( const std::string & name,
- const SotH * ref,
- unsigned int nJ,
- sotRotationComposedInExtenso& Qh,
- bubMatrix &Rh,
- SolverHierarchicalInequalities::ConstraintList &cH )
- :Entity(name),referenceKey(ref),solver(nJ,Qh,Rh,cH)
- ,jacobianConstrainedSINOUT( "sotTaskAbstract("+name+")::inout(matrix)::JK" )
- ,diffErrorSINOUT( "sotTaskAbstract("+name+")::inout(vector)::diff" )
-{ signalRegistration(jacobianConstrainedSINOUT<<diffErrorSINOUT); }
+void SotH::MemoryTaskSOTH::display(std::ostream & /*os*/) const {} // TODO
+
+SotH::MemoryTaskSOTH::MemoryTaskSOTH(
+ const std::string &name, const SotH *ref, unsigned int nJ,
+ sotRotationComposedInExtenso &Qh, bubMatrix &Rh,
+ SolverHierarchicalInequalities::ConstraintList &cH)
+ : Entity(name), referenceKey(ref), solver(nJ, Qh, Rh, cH),
+ jacobianConstrainedSINOUT("sotTaskAbstract(" + name +
+ ")::inout(matrix)::JK"),
+ diffErrorSINOUT("sotTaskAbstract(" + name + ")::inout(vector)::diff") {
+ signalRegistration(jacobianConstrainedSINOUT << diffErrorSINOUT);
+}
diff --git a/src/sot/sot-qr.cpp b/src/sot/sot-qr.cpp
index 86c3738d..31038e30 100644
--- a/src/sot/sot-qr.cpp
+++ b/src/sot/sot-qr.cpp
@@ -13,20 +13,19 @@
/* SOT */
#include <sot/core/debug.hh>
-class sotSOTQr__INIT
-{
-public:sotSOTQr__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
+class sotSOTQr__INIT {
+public:
+ sotSOTQr__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
sotSOTQr__INIT sotSOTQr_initiator;
-
-#include <sot/core/sot-qr.hh>
+#include <sot/core/memory-task-sot.hh>
#include <sot/core/pool.hh>
-#include <sot/core/task.hh>
+#include <sot/core/sot-qr.hh>
#include <sot/core/sot.hh>
-#include <sot/core/memory-task-sot.hh>
+#include <sot/core/task.hh>
-#define FORTRAN_ID( id ) id##_
+#define FORTRAN_ID(id) id##_
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
@@ -35,143 +34,146 @@ using namespace dynamicgraph;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotQr,"SOTQr");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotQr, "SOTQr");
const double SotQr::INVERSION_THRESHOLD_DEFAULT = 1e-4;
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
-SotQr::
-SotQr( const std::string& name )
- :Entity(name)
- ,stack()
- ,constraintList()
- ,ffJointIdFirst( FF_JOINT_ID_DEFAULT )
- ,ffJointIdLast( FF_JOINT_ID_DEFAULT+6 )
-
- ,nbJoints( 0 )
- ,taskGradient(0)
- ,q0SIN( NULL,"sotSOTQr("+name+")::input(double)::q0" )
- ,inversionThresholdSIN( NULL,"sotSOTQr("+name+")::input(double)::damping" )
- ,constraintSOUT( boost::bind(&SotQr::computeConstraintProjector,this,_1,_2),
- sotNOSIGNAL,
- "sotSOTQr("+name+")::output(matrix)::constraint" )
- ,controlSOUT( boost::bind(&SotQr::computeControlLaw,this,_1,_2),
- constraintSOUT<<inversionThresholdSIN<<q0SIN,
- "sotSOTQr("+name+")::output(vector)::control" )
-{
+SotQr::SotQr(const std::string &name)
+ : Entity(name), stack(), constraintList(),
+ ffJointIdFirst(FF_JOINT_ID_DEFAULT),
+ ffJointIdLast(FF_JOINT_ID_DEFAULT + 6)
+
+ ,
+ nbJoints(0), taskGradient(0),
+ q0SIN(NULL, "sotSOTQr(" + name + ")::input(double)::q0"),
+ inversionThresholdSIN(NULL,
+ "sotSOTQr(" + name + ")::input(double)::damping"),
+ constraintSOUT(
+ boost::bind(&SotQr::computeConstraintProjector, this, _1, _2),
+ sotNOSIGNAL, "sotSOTQr(" + name + ")::output(matrix)::constraint"),
+ controlSOUT(boost::bind(&SotQr::computeControlLaw, this, _1, _2),
+ constraintSOUT << inversionThresholdSIN << q0SIN,
+ "sotSOTQr(" + name + ")::output(vector)::control") {
inversionThresholdSIN = INVERSION_THRESHOLD_DEFAULT;
- signalRegistration( inversionThresholdSIN<<controlSOUT<<constraintSOUT<<q0SIN );
+ signalRegistration(inversionThresholdSIN << controlSOUT << constraintSOUT
+ << q0SIN);
}
/* --------------------------------------------------------------------- */
/* --- STACK MANIPULATION --- */
/* --------------------------------------------------------------------- */
-void SotQr::
-push( TaskAbstract& task )
-{
+void SotQr::push(TaskAbstract &task) {
if (nbJoints == 0)
- throw std::logic_error ("Set joint size of "+ getClassName() + " \""+getName()+"\" first");
- stack.push_back( &task );
- controlSOUT.addDependency( task.taskSOUT );
- controlSOUT.addDependency( task.jacobianSOUT );
- //controlSOUT.addDependency( task.featureActivationSOUT );
+ throw std::logic_error("Set joint size of " + getClassName() + " \"" +
+ getName() + "\" first");
+ stack.push_back(&task);
+ controlSOUT.addDependency(task.taskSOUT);
+ controlSOUT.addDependency(task.jacobianSOUT);
+ // controlSOUT.addDependency( task.featureActivationSOUT );
controlSOUT.setReady();
}
-TaskAbstract& SotQr::
-pop( void )
-{
- TaskAbstract* res = stack.back();
+TaskAbstract &SotQr::pop(void) {
+ TaskAbstract *res = stack.back();
stack.pop_back();
- controlSOUT.removeDependency( res->taskSOUT );
- controlSOUT.removeDependency( res->jacobianSOUT );
+ controlSOUT.removeDependency(res->taskSOUT);
+ controlSOUT.removeDependency(res->jacobianSOUT);
controlSOUT.setReady();
return *res;
}
-bool SotQr::
-exist( const TaskAbstract& key )
-{
- std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { return true; }
+bool SotQr::exist(const TaskAbstract &key) {
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ return true;
}
+ }
return false;
}
-void SotQr::
-remove( const TaskAbstract& key )
-{
- bool find =false; std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void SotQr::remove(const TaskAbstract &key) {
+ bool find = false;
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if(! find ){ return; }
+ }
+ if (!find) {
+ return;
+ }
- stack.erase( it );
- removeDependency( key );
+ stack.erase(it);
+ removeDependency(key);
}
-void SotQr::
-removeDependency( const TaskAbstract& key )
-{
- controlSOUT.removeDependency( key.taskSOUT );
- controlSOUT.removeDependency( key.jacobianSOUT );
+void SotQr::removeDependency(const TaskAbstract &key) {
+ controlSOUT.removeDependency(key.taskSOUT);
+ controlSOUT.removeDependency(key.jacobianSOUT);
controlSOUT.setReady();
}
-void SotQr::
-up( const TaskAbstract& key )
-{
- bool find =false; std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void SotQr::up(const TaskAbstract &key) {
+ bool find = false;
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if( stack.begin()==it ) { return; }
- if(! find ){ return; }
+ }
+ if (stack.begin() == it) {
+ return;
+ }
+ if (!find) {
+ return;
+ }
- std::list<TaskAbstract*>::iterator pos=it; pos--;
- TaskAbstract * task = *it;
- stack.erase( it );
- stack.insert( pos,task );
+ std::list<TaskAbstract *>::iterator pos = it;
+ pos--;
+ TaskAbstract *task = *it;
+ stack.erase(it);
+ stack.insert(pos, task);
controlSOUT.setReady();
}
-void SotQr::
-down( const TaskAbstract& key )
-{
- bool find =false; std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void SotQr::down(const TaskAbstract &key) {
+ bool find = false;
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if( stack.end()==it ) { return; }
- if(! find ){ return; }
+ }
+ if (stack.end() == it) {
+ return;
+ }
+ if (!find) {
+ return;
+ }
- std::list<TaskAbstract*>::iterator pos=it; pos++;
- TaskAbstract* task=*it;
- stack.erase( it );
- if( stack.end()==pos ){ stack.push_back(task); }
- else
- {
- pos++;
- stack.insert( pos,task );
- }
+ std::list<TaskAbstract *>::iterator pos = it;
+ pos++;
+ TaskAbstract *task = *it;
+ stack.erase(it);
+ if (stack.end() == pos) {
+ stack.push_back(task);
+ } else {
+ pos++;
+ stack.insert(pos, task);
+ }
controlSOUT.setReady();
}
-void SotQr::
-clear( void )
-{
- for ( std::list<TaskAbstract*>::iterator it=stack.begin();stack.end()!=it;++it )
- {
- removeDependency( **it );
- }
+void SotQr::clear(void) {
+ for (std::list<TaskAbstract *>::iterator it = stack.begin();
+ stack.end() != it; ++it) {
+ removeDependency(**it);
+ }
stack.clear();
controlSOUT.setReady();
}
@@ -180,84 +182,85 @@ clear( void )
/* --- CONSTRAINTS ----------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void SotQr::
-addConstraint( Constraint& constraint )
-{
- constraintList.push_back( &constraint );
- constraintSOUT.addDependency( constraint.jacobianSOUT );
+void SotQr::addConstraint(Constraint &constraint) {
+ constraintList.push_back(&constraint);
+ constraintSOUT.addDependency(constraint.jacobianSOUT);
}
-void SotQr::
-removeConstraint( const Constraint& key )
-{
- bool find =false; ConstraintListType::iterator it;
- for ( it=constraintList.begin();constraintList.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void SotQr::removeConstraint(const Constraint &key) {
+ bool find = false;
+ ConstraintListType::iterator it;
+ for (it = constraintList.begin(); constraintList.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if(! find ){ return; }
+ }
+ if (!find) {
+ return;
+ }
- constraintList.erase( it );
+ constraintList.erase(it);
- constraintSOUT.removeDependency( key.jacobianSOUT );
+ constraintSOUT.removeDependency(key.jacobianSOUT);
}
-void SotQr::
-clearConstraint( void )
-{
- for ( ConstraintListType::iterator it=constraintList.begin();
- constraintList.end()!=it;++it )
- {
- constraintSOUT.removeDependency( (*it)->jacobianSOUT );
- }
+void SotQr::clearConstraint(void) {
+ for (ConstraintListType::iterator it = constraintList.begin();
+ constraintList.end() != it; ++it) {
+ constraintSOUT.removeDependency((*it)->jacobianSOUT);
+ }
constraintList.clear();
}
-void SotQr::
-defineFreeFloatingJoints( const unsigned int& first,const unsigned int& last )
-{
- ffJointIdFirst = first ;
- if( last>0 ) ffJointIdLast=last ;
- else ffJointIdLast=ffJointIdFirst+6;
+void SotQr::defineFreeFloatingJoints(const unsigned int &first,
+ const unsigned int &last) {
+ ffJointIdFirst = first;
+ if (last > 0)
+ ffJointIdLast = last;
+ else
+ ffJointIdLast = ffJointIdFirst + 6;
}
-
/* --------------------------------------------------------------------- */
/* --- CONTROL --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-//std::ostringstream MATLAB;
-class MATLAB
-{
+// std::ostringstream MATLAB;
+class MATLAB {
public:
std::string str;
- friend std::ostream & operator << (std::ostream & os, const MATLAB & m )
- {return os << m.str; }
+ friend std::ostream &operator<<(std::ostream &os, const MATLAB &m) {
+ return os << m.str;
+ }
- MATLAB( const Vector& v1 )
- {
- std::ostringstream os; os << "[ ";
- for( unsigned int i=0;i<v1.size();++i )
- {
- os << v1(i);
- if( v1.size()!=i+1 ) { os << ", "; }
- else { os << "]"; }
+ MATLAB(const Vector &v1) {
+ std::ostringstream os;
+ os << "[ ";
+ for (unsigned int i = 0; i < v1.size(); ++i) {
+ os << v1(i);
+ if (v1.size() != i + 1) {
+ os << ", ";
+ } else {
+ os << "]";
}
+ }
str = os.str();
}
- MATLAB( const Matrix& m1)
- {
- std::ostringstream os; os << "[ ";
- for( unsigned int i=0;i<m1.size1();++i )
- {
- for( unsigned int j=0;j<m1.size2();++j )
- {
- os << m1(i,j);
- if( m1.size2()!=j+1 ) os << ", ";
- }
- if( m1.size1()!=i+1 ) { os << " ;" << std::endl; }
- else { os << " ]"; }
+ MATLAB(const Matrix &m1) {
+ std::ostringstream os;
+ os << "[ ";
+ for (unsigned int i = 0; i < m1.size1(); ++i) {
+ for (unsigned int j = 0; j < m1.size2(); ++j) {
+ os << m1(i, j);
+ if (m1.size2() != j + 1)
+ os << ", ";
}
+ if (m1.size1() != i + 1) {
+ os << " ;" << std::endl;
+ } else {
+ os << " ]";
+ }
+ }
str = os.str();
}
};
@@ -265,58 +268,62 @@ public:
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-#define LAPACK_DGEQP3 FORTRAN_ID( dgeqp3 )
-#define LAPACK_DGEQPF FORTRAN_ID( dgeqpf )
+#define LAPACK_DGEQP3 FORTRAN_ID(dgeqp3)
+#define LAPACK_DGEQPF FORTRAN_ID(dgeqpf)
extern "C" {
- void LAPACK_DGEQP3( const int* m, const int* n, double* a, const int* lda, int * jpvt,
- double* tau, double* work, const int* lwork, int* info );
- void LAPACK_DGEQPF( const int* m, const int* n, double* a, const int* lda, int * jpvt,
- double* tau, double* work, int* info );
+void LAPACK_DGEQP3(const int *m, const int *n, double *a, const int *lda,
+ int *jpvt, double *tau, double *work, const int *lwork,
+ int *info);
+void LAPACK_DGEQPF(const int *m, const int *n, double *a, const int *lda,
+ int *jpvt, double *tau, double *work, int *info);
}
-namespace boost { namespace numeric { namespace bindings { namespace lapack
-{
+namespace boost {
+namespace numeric {
+namespace bindings {
+namespace lapack {
+
+inline int geqp(bub::matrix<double, bub::column_major> &A, bub::vector<int> &jp,
+ Vector &tau) {
+#ifndef NDEBUG
+ const int m = A.size1();
+#endif
+ const int n = A.size2();
+ ::boost::numeric::ublas::vector<double> work(std::max(1, n * 32));
+
+ assert(std::min(m, n) <= tau.size());
+ assert(n <= work.size());
+
+ int const mF = A.size1();
+ int const nF = A.size2();
+ double *aF = MRAWDATA(A);
+ int const ldaF = traits::leading_dimension(A);
+ int *jpvtF = VRAWDATA(jp);
+ double *tauF = VRAWDATA(tau);
+ double *workF = VRAWDATA(work);
+ int const lworkF = work.size();
+ int infoF;
+
+ // std::cout<<"lda = " << ldaF << std::endl;
+ // std::cout << "mF = " << mF << std::endl;
+ // std::cout << "nF = " << nF << std::endl;
+ // //std::cout << "aF = " << aF << std::endl;
+ // std::cout << "ldaF = " << ldaF << std::endl;
+ // //std::cout << "jpvtF = " << jpvtF << std::endl;
+ // //std::cout << "tauF = " << tauF << std::endl;
+ // //std::cout << "workF = " << workF << std::endl;
+ // std::cout << "lworkF = " << lworkF << std::endl;
+ // std::cout << "infoF " << infoF << std::endl;
+
+ LAPACK_DGEQP3(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &lworkF, &infoF);
+ // LAPACK_DGEQPF(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &infoF);
+ return infoF;
+}
- inline int geqp (bub::matrix<double,bub::column_major> &A,
- bub::vector< int >& jp, Vector& tau)
- {
- #ifndef NDEBUG
- const int m = A.size1();
- #endif
- const int n = A.size2();
- ::boost::numeric::ublas::vector<double> work(std::max(1, n*32));
-
- assert (std::min(m,n) <= tau.size());
- assert (n <= work.size());
-
- int const mF= A.size1();
- int const nF= A.size2();
- double* aF = MRAWDATA(A);
- int const ldaF = traits::leading_dimension (A);
- int * jpvtF = VRAWDATA(jp);
- double * tauF = VRAWDATA(tau);
- double * workF = VRAWDATA(work);
- int const lworkF = work.size();
- int infoF;
-
- // std::cout<<"lda = " << ldaF << std::endl;
- // std::cout << "mF = " << mF << std::endl;
- // std::cout << "nF = " << nF << std::endl;
- // //std::cout << "aF = " << aF << std::endl;
- // std::cout << "ldaF = " << ldaF << std::endl;
- // //std::cout << "jpvtF = " << jpvtF << std::endl;
- // //std::cout << "tauF = " << tauF << std::endl;
- // //std::cout << "workF = " << workF << std::endl;
- // std::cout << "lworkF = " << lworkF << std::endl;
- // std::cout << "infoF " << infoF << std::endl;
-
-
- LAPACK_DGEQP3(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &lworkF, &infoF);
- //LAPACK_DGEQPF(&mF, &nF, aF, &ldaF, jpvtF, tauF, workF, &infoF);
- return infoF;
- }
-
- }}}}
+} // namespace lapack
+} // namespace bindings
+} // namespace numeric
+} // namespace boost
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
@@ -330,188 +337,195 @@ namespace boost { namespace numeric { namespace bindings { namespace lapack
* given rotation.
*/
-
/* Xn gives the range of the vector to consider when Rx = [ R X ] = [ R Y x Z ].
* In that case, x=X(:,xn), starting from 0 (xn=0 -> x is the first
* column of X).
* TODO: optimize, considering that R is triangular!
*/
-template< typename bubTemplateMatrix >
-void invCholeskiUpdate( bubTemplateMatrix & Rx, const unsigned int xn=0 )
-{
+template <typename bubTemplateMatrix>
+void invCholeskiUpdate(bubTemplateMatrix &Rx, const unsigned int xn = 0) {
const unsigned int n = Rx.size1();
- double c,s,tau;
- for( unsigned int iter =0; iter<n;iter++ )
- {
- //std::cout << "Rx" << iter << " = " << (MATLAB)Rx << std::endl << std::endl;
-
- const unsigned int i=n-iter-1;
- const unsigned int j=n-iter-1;
- const unsigned int k=n+xn;
-
- const double &a=Rx(j,i);
- const double &b=Rx(j,k);
- if( fabs(b)>fabs(a) )
- { tau=-a/b; s=1/sqrt(1+tau*tau); c=s*tau; }
- else
- { tau=-b/a; c=1/sqrt(1+tau*tau); s=c*tau; }
-
- for( unsigned int row=0;row<n-iter;++row )
- {
- const double ri = Rx(row,i);
- const double rk = Rx(row,k);
- Rx(row,i) = ri*c-rk*s;
- Rx(row,k) = ri*s+rk*c;
- }
+ double c, s, tau;
+ for (unsigned int iter = 0; iter < n; iter++) {
+ // std::cout << "Rx" << iter << " = " << (MATLAB)Rx << std::endl <<
+ // std::endl;
+
+ const unsigned int i = n - iter - 1;
+ const unsigned int j = n - iter - 1;
+ const unsigned int k = n + xn;
+
+ const double &a = Rx(j, i);
+ const double &b = Rx(j, k);
+ if (fabs(b) > fabs(a)) {
+ tau = -a / b;
+ s = 1 / sqrt(1 + tau * tau);
+ c = s * tau;
+ } else {
+ tau = -b / a;
+ c = 1 / sqrt(1 + tau * tau);
+ s = c * tau;
+ }
+
+ for (unsigned int row = 0; row < n - iter; ++row) {
+ const double ri = Rx(row, i);
+ const double rk = Rx(row, k);
+ Rx(row, i) = ri * c - rk * s;
+ Rx(row, k) = ri * s + rk * c;
}
- //std::cout << "Rt = " << (MATLAB)Rx << std::endl << std::endl;
+ }
+ // std::cout << "Rt = " << (MATLAB)Rx << std::endl << std::endl;
}
/* Same, but with a collection of vector instead of only one. */
-template< typename bubTemplateMatrix >
-void invGeneralizeCholeskiUpdate( bubTemplateMatrix & Rx )
-{
+template <typename bubTemplateMatrix>
+void invGeneralizeCholeskiUpdate(bubTemplateMatrix &Rx) {
const unsigned int n = Rx.size1();
- const unsigned int xn = Rx.size2()-n;
- //std::cout << "Cho init " << Rx<< std::endl;
- //std::cout << "Cho init " << n << ":" << xn << std::endl;
- for( unsigned int i=0;i<xn;++i )
- {
- invCholeskiUpdate(Rx,i);
- }
+ const unsigned int xn = Rx.size2() - n;
+ // std::cout << "Cho init " << Rx<< std::endl;
+ // std::cout << "Cho init " << n << ":" << xn << std::endl;
+ for (unsigned int i = 0; i < xn; ++i) {
+ invCholeskiUpdate(Rx, i);
+ }
}
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-class sotHouseholdMatrix
-{
+class sotHouseholdMatrix {
public: // protected:
Vector v;
double beta;
public:
- sotHouseholdMatrix( void ) { v.resize(0);}
- sotHouseholdMatrix( const unsigned int n )
- { v.resize(n); std::fill(v.begin(),v.end(),0); beta=0;}
- sotHouseholdMatrix( const Vector& _v, const double& _beta )
- { v= _v; beta=_beta; }
- sotHouseholdMatrix( const Matrix& _v, const double& _beta )
- {
+ sotHouseholdMatrix(void) { v.resize(0); }
+ sotHouseholdMatrix(const unsigned int n) {
+ v.resize(n);
+ std::fill(v.begin(), v.end(), 0);
+ beta = 0;
+ }
+ sotHouseholdMatrix(const Vector &_v, const double &_beta) {
+ v = _v;
+ beta = _beta;
+ }
+ sotHouseholdMatrix(const Matrix &_v, const double &_beta) {
v.resize(_v.size1());
- for( unsigned int i=0;i<_v.size1();++i )
- {v(i)= _v(i,0);}
- beta=_beta;
+ for (unsigned int i = 0; i < _v.size1(); ++i) {
+ v(i) = _v(i, 0);
+ }
+ beta = _beta;
}
public:
-
/* TODO: vstar is computed with many 0. same for vstar^T.
* The computation should be optimized using only A22 instead of
* A = [A11 A12; A21 A22]; */
- void multiplyRight( Vector& a ) const // P*a
+ void multiplyRight(Vector &a) const // P*a
{
- const unsigned int m=a.size();
+ const unsigned int m = a.size();
const unsigned int kv = v.size();
- const unsigned int m_kv = m-kv;
+ const unsigned int m_kv = m - kv;
- double w; w=0;
+ double w;
+ w = 0;
// w <- b v' A = b [ 0 ... 0 1 v' ] a
- for( unsigned int i=0;i<kv;++i )
- { w += v(i)*a(i+m_kv); }
- w*=beta;
+ for (unsigned int i = 0; i < kv; ++i) {
+ w += v(i) * a(i + m_kv);
+ }
+ w *= beta;
// a <- a - v w = [ 0; ... ; 0; 1; v ] w
- for( unsigned int i=0;i<kv;++i )
- { a(i+m_kv)-=v(i)*w; }
+ for (unsigned int i = 0; i < kv; ++i) {
+ a(i + m_kv) -= v(i) * w;
+ }
}
- void multiplyRight( Matrix& A ) const // P*A
+ void multiplyRight(Matrix &A) const // P*A
{
- const unsigned int m=A.size1(), n=A.size2();
+ const unsigned int m = A.size1(), n = A.size2();
const unsigned int kv = v.size();
- const unsigned int m_kv = m-kv;
+ const unsigned int m_kv = m - kv;
- Vector w(n); w*=0;
+ Vector w(n);
+ w *= 0;
// W <- b v' A = b [ 0 ... 0 1 v' ] A
- for( unsigned int j=0;j<n;++j )
- {
- for( unsigned int i=0;i<kv;++i )
- { w(j) += v(i)*A(i+m_kv,j); }
- w(j)*=beta;
+ for (unsigned int j = 0; j < n; ++j) {
+ for (unsigned int i = 0; i < kv; ++i) {
+ w(j) += v(i) * A(i + m_kv, j);
}
+ w(j) *= beta;
+ }
// A <- A - v w = [ 0; ... ; 0; 1; v ] w
- for( unsigned int i=0;i<kv;++i )
- for( unsigned int j=0;j<n;++j )
- { A(i+m_kv,j)-=v(i)*w(j); }
+ for (unsigned int i = 0; i < kv; ++i)
+ for (unsigned int j = 0; j < n; ++j) {
+ A(i + m_kv, j) -= v(i) * w(j);
+ }
}
-
- friend std::ostream & operator << ( std::ostream & os,const sotHouseholdMatrix & P )
- {
+ friend std::ostream &operator<<(std::ostream &os,
+ const sotHouseholdMatrix &P) {
return os << "[b=" << P.beta << "] " << (MATLAB)P.v;
}
};
-class sotHouseholdMatrices
-{
+class sotHouseholdMatrices {
public: // protected:
typedef std::list<sotHouseholdMatrix> HouseholdList;
HouseholdList matrices;
static const double THRESHOLD;
public:
- sotHouseholdMatrices( void ) { matrices.resize(0);}
- sotHouseholdMatrices( const Matrix & RQ, const Vector & betas )
- { this->push_back(RQ,betas); }
+ sotHouseholdMatrices(void) { matrices.resize(0); }
+ sotHouseholdMatrices(const Matrix &RQ, const Vector &betas) {
+ this->push_back(RQ, betas);
+ }
- void push_back( const sotHouseholdMatrix& P )
- { matrices.push_back( P ); }
+ void push_back(const sotHouseholdMatrix &P) { matrices.push_back(P); }
/* <nbVector> is the number of vector to consider. If nbVector<0, all
* the householder vectors are added. */
- void push_back( const bub::matrix<double> & RQ,
- const Vector & betas,
- const int nbVector=-1 )
- {
- const unsigned int nToProceed
- =((nbVector<0)
- ?std::min(RQ.size1(),RQ.size2())
- :(unsigned int)nbVector);
- const unsigned int m=RQ.size1();
-
- //unsigned int i=0;
- //while( (i<n)&&(fabs(betas(i))>THRESHOLD) )
-
- for( unsigned int i=0;i<nToProceed;++i )
-
- {
- Vector v(m-i); v(0)=1;
- for( unsigned int j=1;j<m-i;++j ) v(j)=RQ(j+i,i);
- sotHouseholdMatrix(v,betas(i));
- this->push_back( sotHouseholdMatrix(v,betas(i)) );
- }
+ void push_back(const bub::matrix<double> &RQ, const Vector &betas,
+ const int nbVector = -1) {
+ const unsigned int nToProceed =
+ ((nbVector < 0) ? std::min(RQ.size1(), RQ.size2())
+ : (unsigned int)nbVector);
+ const unsigned int m = RQ.size1();
+
+ // unsigned int i=0;
+ // while( (i<n)&&(fabs(betas(i))>THRESHOLD) )
+
+ for (unsigned int i = 0; i < nToProceed; ++i)
+
+ {
+ Vector v(m - i);
+ v(0) = 1;
+ for (unsigned int j = 1; j < m - i; ++j)
+ v(j) = RQ(j + i, i);
+ sotHouseholdMatrix(v, betas(i));
+ this->push_back(sotHouseholdMatrix(v, betas(i)));
+ }
}
- void push_back( const sotHouseholdMatrices& Q2 )
- {
- for( HouseholdList::const_iterator Pi = Q2.matrices.begin();
- Pi!=Q2.matrices.end(); ++Pi )
- { matrices.push_back( *Pi ); }
+ void push_back(const sotHouseholdMatrices &Q2) {
+ for (HouseholdList::const_iterator Pi = Q2.matrices.begin();
+ Pi != Q2.matrices.end(); ++Pi) {
+ matrices.push_back(*Pi);
+ }
}
/* --- Vectors --- */
- virtual void multiplyRight( Vector& a ) const // Q*a = P1*P2*...*Pn*a
+ virtual void multiplyRight(Vector &a) const // Q*a = P1*P2*...*Pn*a
{
- for( HouseholdList::const_reverse_iterator Pi = matrices.rbegin();
- Pi!=matrices.rend(); ++Pi )
- { Pi->multiplyRight(a); }
+ for (HouseholdList::const_reverse_iterator Pi = matrices.rbegin();
+ Pi != matrices.rend(); ++Pi) {
+ Pi->multiplyRight(a);
+ }
}
- virtual void multiplyTransposeRight( Vector& a ) const // Q*a = Pn*...*P2*P1*a
+ virtual void multiplyTransposeRight(Vector &a) const // Q*a = Pn*...*P2*P1*a
{
- for( HouseholdList::const_iterator Pi=matrices.begin();
- Pi!=matrices.end(); ++Pi )
- { Pi->multiplyRight(a); }
+ for (HouseholdList::const_iterator Pi = matrices.begin();
+ Pi != matrices.end(); ++Pi) {
+ Pi->multiplyRight(a);
+ }
}
/* For both sub range product, I tried to optimize a little bit
@@ -520,79 +534,85 @@ public:
* the very first product. All the other P's have to be multiply in full.
* Consequently, I will not waste any more time to save some 4 or 5
* double product. */
- virtual void multiplyRangeRight( const Vector& a, // N*A = Q*[0;I]*A = P1*...*Pn*[0;A]
- Vector& res,
- const unsigned int zeroBefore,
- const unsigned int zeroAfter ) const
- {
- const unsigned int m=a.size(); //n =1;
- const unsigned int mpp=m+zeroBefore+zeroAfter;
- Vector &app = res; app.resize( mpp ); app.assign(bub::zero_vector<double>(mpp));
- bub::vector_range< Vector > acopy(app,bub::range(zeroBefore,zeroBefore+m));
+ virtual void
+ multiplyRangeRight(const Vector &a, // N*A = Q*[0;I]*A = P1*...*Pn*[0;A]
+ Vector &res, const unsigned int zeroBefore,
+ const unsigned int zeroAfter) const {
+ const unsigned int m = a.size(); // n =1;
+ const unsigned int mpp = m + zeroBefore + zeroAfter;
+ Vector &app = res;
+ app.resize(mpp);
+ app.assign(bub::zero_vector<double>(mpp));
+ bub::vector_range<Vector> acopy(app,
+ bub::range(zeroBefore, zeroBefore + m));
acopy.assign(a);
multiplyRight(app);
}
/* Same as before, no optimization done here. */
// N'*a = [0 I]*Q'*a = subrange( P1*...*Pn*A )
- virtual void multiplyTransposeRangeRight( const Vector& a,
- Vector& res,
- const unsigned int zeroBefore,
- const unsigned int zeroAfter ) const
- {
- Vector acopy = a; multiplyTransposeRight(acopy);
+ virtual void multiplyTransposeRangeRight(const Vector &a, Vector &res,
+ const unsigned int zeroBefore,
+ const unsigned int zeroAfter) const {
+ Vector acopy = a;
+ multiplyTransposeRight(acopy);
const unsigned int mres = a.size() - zeroBefore - zeroAfter;
- //bub::matrix_range< Matrix > asub (acopy,bub::range(zeroBefore,mres));
- res.resize(mres); res.assign( bub::project( acopy,bub::range(zeroBefore,mres)) );
+ // bub::matrix_range< Matrix > asub (acopy,bub::range(zeroBefore,mres));
+ res.resize(mres);
+ res.assign(bub::project(acopy, bub::range(zeroBefore, mres)));
}
/* --- Matrices --- */
- virtual void multiplyRight( Matrix& A ) const // Q*A = P1*P2*...*Pn*A
+ virtual void multiplyRight(Matrix &A) const // Q*A = P1*P2*...*Pn*A
{
- for( HouseholdList::const_reverse_iterator Pi = matrices.rbegin();
- Pi!=matrices.rend(); ++Pi )
- { Pi->multiplyRight(A); }
+ for (HouseholdList::const_reverse_iterator Pi = matrices.rbegin();
+ Pi != matrices.rend(); ++Pi) {
+ Pi->multiplyRight(A);
+ }
}
- virtual void multiplyTransposeRight( Matrix& A ) const // Q*A = Pn*...*P2*P1*A
+ virtual void multiplyTransposeRight(Matrix &A) const // Q*A = Pn*...*P2*P1*A
{
- for( HouseholdList::const_iterator Pi = matrices.begin();
- Pi!=matrices.end(); ++Pi )
- { Pi->multiplyRight(A); }
+ for (HouseholdList::const_iterator Pi = matrices.begin();
+ Pi != matrices.end(); ++Pi) {
+ Pi->multiplyRight(A);
+ }
}
- friend std::ostream & operator << ( std::ostream & os,const sotHouseholdMatrices & Ps )
- {
- for( HouseholdList::const_iterator Pi = Ps.matrices.begin();
- Pi!=Ps.matrices.end(); ++Pi )
- {
- os << (*Pi) << std::endl;
- }
- if( Ps.matrices.empty() ) os << "Identity" << std::endl;
+ friend std::ostream &operator<<(std::ostream &os,
+ const sotHouseholdMatrices &Ps) {
+ for (HouseholdList::const_iterator Pi = Ps.matrices.begin();
+ Pi != Ps.matrices.end(); ++Pi) {
+ os << (*Pi) << std::endl;
+ }
+ if (Ps.matrices.empty())
+ os << "Identity" << std::endl;
return os;
}
-
};
-
-
const double sotHouseholdMatrices::THRESHOLD = 1e-9;
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-typedef bub::triangular_adaptor< bub::matrix_range< bub::matrix<double,boost::numeric::ublas::column_major> >, bub::upper> MatrixTriSquare;
+typedef bub::triangular_adaptor<
+ bub::matrix_range<bub::matrix<double, boost::numeric::ublas::column_major>>,
+ bub::upper>
+ MatrixTriSquare;
/* Assuming a diagonal-ordered triangular matrix. */
-unsigned int rankDetermination( const Matrix& A,
- const double threshold = 1e-6 )
-{
- const unsigned int n = traits::leading_dimension (A);
+unsigned int rankDetermination(const Matrix &A, const double threshold = 1e-6) {
+ const unsigned int n = traits::leading_dimension(A);
unsigned int res = 0;
- for( unsigned int i=0;i<n;++i )
- { if( fabs(A(i,i))>threshold ) res++; else break; }
+ for (unsigned int i = 0; i < n; ++i) {
+ if (fabs(A(i, i)) > threshold)
+ res++;
+ else
+ break;
+ }
return res;
}
@@ -602,210 +622,219 @@ unsigned int rankDetermination( const Matrix& A,
#define WITH_CHRONO
-
-#ifdef WITH_CHRONO
- #ifndef WIN32
- #include <sys/time.h>
- #else /*WIN32*/
- #include <sot/core/utils-windows.hh>
- #endif /*WIN32*/
+#ifdef WITH_CHRONO
+#ifndef WIN32
+#include <sys/time.h>
+#else /*WIN32*/
+#include <sot/core/utils-windows.hh>
+#endif /*WIN32*/
#endif /*WITH_CHRONO*/
-
-#ifdef WITH_CHRONO
-# define sotINIT_CHRONO1 struct timeval t0,t1; double dt
-# define sotSTART_CHRONO1 gettimeofday(&t0,NULL)
-# define sotCHRONO1 \
- gettimeofday(&t1,NULL);\
- dt = ( (t1.tv_sec-t0.tv_sec) * 1000.* 1000.\
- + (t1.tv_usec-t0.tv_usec+0.) );\
- sotDEBUG(1) << "dt: "<< dt / 1000. << std::endl
-# define sotINITPARTCOUNTERS struct timeval tpart0
-# define sotSTARTPARTCOUNTERS gettimeofday(&tpart0,NULL)
-# define sotCOUNTER(nbc1,nbc2) \
- gettimeofday(&tpart##nbc2,NULL); \
- dt##nbc2 += ( (tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000.* 1000. \
- + (tpart##nbc2.tv_usec-tpart##nbc1.tv_usec+0.) )
-# define sotINITCOUNTER(nbc1) \
- struct timeval tpart##nbc1; double dt##nbc1=0;
-# define sotPRINTCOUNTER(nbc1) sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
+#ifdef WITH_CHRONO
+#define sotINIT_CHRONO1 \
+ struct timeval t0, t1; \
+ double dt
+#define sotSTART_CHRONO1 gettimeofday(&t0, NULL)
+#define sotCHRONO1 \
+ gettimeofday(&t1, NULL); \
+ dt = ((t1.tv_sec - t0.tv_sec) * 1000. * 1000. + \
+ (t1.tv_usec - t0.tv_usec + 0.)); \
+ sotDEBUG(1) << "dt: " << dt / 1000. << std::endl
+#define sotINITPARTCOUNTERS struct timeval tpart0
+#define sotSTARTPARTCOUNTERS gettimeofday(&tpart0, NULL)
+#define sotCOUNTER(nbc1, nbc2) \
+ gettimeofday(&tpart##nbc2, NULL); \
+ dt##nbc2 += ((tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1000. * 1000. + \
+ (tpart##nbc2.tv_usec - tpart##nbc1.tv_usec + 0.))
+#define sotINITCOUNTER(nbc1) \
+ struct timeval tpart##nbc1; \
+ double dt##nbc1 = 0;
+#define sotPRINTCOUNTER(nbc1) \
+ sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
#else // #ifdef WITH_CHRONO
-# define sotINIT_CHRONO1
-# define sotSTART_CHRONO1
-# define sotCHRONO1
-# define sotINITPARTCOUNTERS
-# define sotSTARTPARTCOUNTERS
-# define sotCOUNTER(nbc1,nbc2)
-# define sotINITCOUNTER(nbc1)
-# define sotPRINTCOUNTER(nbc1)
+#define sotINIT_CHRONO1
+#define sotSTART_CHRONO1
+#define sotCHRONO1
+#define sotINITPARTCOUNTERS
+#define sotSTARTPARTCOUNTERS
+#define sotCOUNTER(nbc1, nbc2)
+#define sotINITCOUNTER(nbc1)
+#define sotPRINTCOUNTER(nbc1)
#endif // #ifdef WITH_CHRONO
-dynamicgraph::Vector& SotQr::
-computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
-{
+dynamicgraph::Vector &SotQr::computeControlLaw(dynamicgraph::Vector &control,
+ const int &iterTime) {
sotDEBUGIN(15);
- //sotINIT_CHRONO1;
- sotINITPARTCOUNTERS; sotINITCOUNTER(1); sotINITCOUNTER(2);sotINITCOUNTER(3);
+ // sotINIT_CHRONO1;
+ sotINITPARTCOUNTERS;
+ sotINITCOUNTER(1);
+ sotINITCOUNTER(2);
+ sotINITCOUNTER(3);
- //sotSTART_CHRONO1;
+ // sotSTART_CHRONO1;
sotSTARTPARTCOUNTERS;
- //const double &th = inversionThresholdSIN(iterTime);
+ // const double &th = inversionThresholdSIN(iterTime);
const dynamicgraph::Matrix &K = constraintSOUT(iterTime);
const unsigned int nJ = K.cols();
sotHouseholdMatrices Q;
unsigned int freeRank = nJ;
- Vector u(nJ); u.assign( bub::zero_vector<double>(nJ) );
+ Vector u(nJ);
+ u.assign(bub::zero_vector<double>(nJ));
- sotDEBUGF(5, " --- Time %d -------------------", iterTime );
+ sotDEBUGF(5, " --- Time %d -------------------", iterTime);
/* First stage. */
{
- TaskAbstract & task = **(stack.begin());
+ TaskAbstract &task = **(stack.begin());
const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
const dynamicgraph::Vector err;
Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
const unsigned int mJ = Jac.rows();
- /***/sotCOUNTER(0,1); // Direct Dynamic
+ /***/ sotCOUNTER(0, 1); // Direct Dynamic
/* --- INIT MEMORY --- */
- MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
- if( NULL==mem )
- {
- if( NULL!=task.memoryInternal ) delete task.memoryInternal;
- mem = new MemoryTaskSOT( task.getName()+"_memSOT",nJ,mJ );
- task.memoryInternal = mem;
- }
- /***/sotCOUNTER(1,2); // Direct Dynamic
+ MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(task.memoryInternal);
+ if (NULL == mem) {
+ if (NULL != task.memoryInternal)
+ delete task.memoryInternal;
+ mem = new MemoryTaskSOT(task.getName() + "_memSOT", nJ, mJ);
+ task.memoryInternal = mem;
+ }
+ /***/ sotCOUNTER(1, 2); // Direct Dynamic
/* --- COMPUTE JK --- */
dynamicgraph::Matrix &JK = mem->JK;
- JK.resize( mJ,nJ );
- mem->Jff.resize( mJ,Jac.cols()-nJ );
- mem->Jact.resize( mJ,nJ );
- Sot::computeJacobianConstrained( task,K );
- /***/sotCOUNTER(2,3); // compute JK
+ JK.resize(mJ, nJ);
+ mem->Jff.resize(mJ, Jac.cols() - nJ);
+ mem->Jact.resize(mJ, nJ);
+ Sot::computeJacobianConstrained(task, K);
+ /***/ sotCOUNTER(2, 3); // compute JK
- const Matrix & J1 = JK.accessToMotherLib();
- const Vector & e1 = err.accessToMotherLib();
- const unsigned int & m1 = e1.size();
+ const Matrix &J1 = JK.accessToMotherLib();
+ const Vector &e1 = err.accessToMotherLib();
+ const unsigned int &m1 = e1.size();
/* --- Compute the QR decomposition. --- */
- Matrix QR1(nJ,m1);
+ Matrix QR1(nJ, m1);
Vector beta1(m1);
Eigen::VectorXi perm1(m1);
QR1 = bub::trans(J1);
- boost::numeric::bindings::lapack::geqp(QR1,perm1,beta1);
- const unsigned int & rank1 = rankDetermination(QR1);
+ boost::numeric::bindings::lapack::geqp(QR1, perm1, beta1);
+ const unsigned int &rank1 = rankDetermination(QR1);
/* --- Organize into Q and R. --- */
/* If J1 is rank deficient, then the last <m-r> householder vectors
* do not affect the range basis M (where Q=[M N]). If M is constant
* wrt these vector, then Span(N)=orth(Span(M)) is also constant, even
* if N is non constant, and thus these vectors can be neglect. */
- Q.push_back( QR1,beta1,rank1 );
- if( rank1<m1 ) // Rank deficiency
- {
- bub::matrix_range< bub::matrix<double,boost::numeric::ublas::column_major> >
- QR1sup( QR1,bub::range(0,rank1),bub::range(0,m1) );
- invGeneralizeCholeskiUpdate( QR1sup );
- }
- bub::matrix_range< bub::matrix<double,boost::numeric::ublas::column_major> >
- QR1sup( QR1,bub::range(0,rank1),bub::range(0,rank1) );
+ Q.push_back(QR1, beta1, rank1);
+ if (rank1 < m1) // Rank deficiency
+ {
+ bub::matrix_range<
+ bub::matrix<double, boost::numeric::ublas::column_major>>
+ QR1sup(QR1, bub::range(0, rank1), bub::range(0, m1));
+ invGeneralizeCholeskiUpdate(QR1sup);
+ }
+ bub::matrix_range<bub::matrix<double, boost::numeric::ublas::column_major>>
+ QR1sup(QR1, bub::range(0, rank1), bub::range(0, rank1));
const MatrixTriSquare R1(QR1sup);
/* --- Compute the control law. --- */
Vector b1(nJ);
/* b <- J'e */
- bub::axpy_prod( e1,J1,b1,true );
+ bub::axpy_prod(e1, J1, b1, true);
/* b <- Q' J' e */
Q.multiplyTransposeRight(b1);
/* Qe1 <- M' J' e */
- bub::vector_range<Vector> Qe1(b1,bub::range(0,rank1));
+ bub::vector_range<Vector> Qe1(b1, bub::range(0, rank1));
/* Qe1 <- R-1 M' J' e */
- bub::lu_substitute(R1,Qe1);
+ bub::lu_substitute(R1, Qe1);
/* Qe1 <- R'-1 R-1 M' J' e */
- bub::lu_substitute(Qe1,(const Matrix &)R1);
+ bub::lu_substitute(Qe1, (const Matrix &)R1);
/* b <- [ Qe1; 0 ] */
- bub::vector_range<Vector> Ne1(b1,bub::range(rank1,nJ));
- Ne1.assign( bub::zero_vector<double>( nJ-rank1-1 ) );
+ bub::vector_range<Vector> Ne1(b1, bub::range(rank1, nJ));
+ Ne1.assign(bub::zero_vector<double>(nJ - rank1 - 1));
/* b <- Q [ Qe1;0] = M R'-1 R-1 M' J' e */
Q.multiplyRight(b1);
/* Ready for the next step... */
- u+=b1;
- freeRank-=rank1;
+ u += b1;
+ freeRank -= rank1;
/* --- Traces --- */
sotDEBUG(45) << " * --- Stage 0 ----------------------- *" << std::endl;
- sotDEBUG(15) << "J1 = " << (MATLAB)J1 << std::endl;
- sotDEBUG(15) << "e1 = " << (MATLAB)e1 << std::endl;
- sotDEBUG(15) << "rank1 = " << rank1 << std::endl;
- sotDEBUG(45) << "Q1 = " << Q;
- sotDEBUG(15) << "R1 = " << (MATLAB)R1 << std::endl;
- sotDEBUG(45) << "du1 = " << (MATLAB)b1 << std::endl;
- sotDEBUG(15) << "u1 = " << (MATLAB)u << std::endl;
+ sotDEBUG(15) << "J1 = " << (MATLAB)J1 << std::endl;
+ sotDEBUG(15) << "e1 = " << (MATLAB)e1 << std::endl;
+ sotDEBUG(15) << "rank1 = " << rank1 << std::endl;
+ sotDEBUG(45) << "Q1 = " << Q;
+ sotDEBUG(15) << "R1 = " << (MATLAB)R1 << std::endl;
+ sotDEBUG(45) << "du1 = " << (MATLAB)b1 << std::endl;
+ sotDEBUG(15) << "u1 = " << (MATLAB)u << std::endl;
}
unsigned int stage = 1;
StackType::iterator iter = stack.begin();
- for( iter++; iter!=stack.end();++iter,stage++ )
- {
- sotDEBUG(45) << " * --- Stage " << stage << " ----------------------- *" << std::endl;
-
- TaskAbstract & task = **iter;
- const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
- const dynamicgraph::Vector err;
- Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
- const unsigned int mJ = Jac.rows();
- /***/sotCOUNTER(0,1); // Direct Dynamic
-
- /* --- INIT MEMORY --- */
- MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
- if( NULL==mem )
- {
- if( NULL!=task.memoryInternal ) delete task.memoryInternal;
- mem = new MemoryTaskSOT( task.getName()+"_memSOT",nJ,mJ );
- task.memoryInternal = mem;
- }
- dynamicgraph::Matrix &JK = mem->JK;
-
- /* --- COMPUTE JK --- */
- JK.resize( mJ,nJ );
- mem->Jff.resize( mJ,Jac.cols()-nJ );
- mem->Jact.resize( mJ,nJ );
- Sot::computeJacobianConstrained( task,K );
- /***/sotCOUNTER(2,3); // compute JK
-
- const Matrix & J2 = JK.accessToMotherLib();
- const Vector & e2 = err.accessToMotherLib();
- const unsigned int & m2 = e2.size();
-
- /* --- Compute the limited matrix. --- */
- Matrix QJt2( nJ,m2 ); QJt2.assign( bub::trans(J2) );
- Q.multiplyTransposeRight(QJt2);
- bub::matrix_range<Matrix> Jt2( QJt2,bub::range(nJ-freeRank,nJ),bub::range(0,m2));
+ for (iter++; iter != stack.end(); ++iter, stage++) {
+ sotDEBUG(45) << " * --- Stage " << stage << " ----------------------- *"
+ << std::endl;
+
+ TaskAbstract &task = **iter;
+ const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
+ const dynamicgraph::Vector err;
+ Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
+ const unsigned int mJ = Jac.rows();
+ /***/ sotCOUNTER(0, 1); // Direct Dynamic
+
+ /* --- INIT MEMORY --- */
+ MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(task.memoryInternal);
+ if (NULL == mem) {
+ if (NULL != task.memoryInternal)
+ delete task.memoryInternal;
+ mem = new MemoryTaskSOT(task.getName() + "_memSOT", nJ, mJ);
+ task.memoryInternal = mem;
+ }
+ dynamicgraph::Matrix &JK = mem->JK;
+
+ /* --- COMPUTE JK --- */
+ JK.resize(mJ, nJ);
+ mem->Jff.resize(mJ, Jac.cols() - nJ);
+ mem->Jact.resize(mJ, nJ);
+ Sot::computeJacobianConstrained(task, K);
+ /***/ sotCOUNTER(2, 3); // compute JK
+
+ const Matrix &J2 = JK.accessToMotherLib();
+ const Vector &e2 = err.accessToMotherLib();
+ const unsigned int &m2 = e2.size();
+
+ /* --- Compute the limited matrix. --- */
+ Matrix QJt2(nJ, m2);
+ QJt2.assign(bub::trans(J2));
+ Q.multiplyTransposeRight(QJt2);
+ bub::matrix_range<Matrix> Jt2(QJt2, bub::range(nJ - freeRank, nJ),
+ bub::range(0, m2));
/* --- Compute the QR decomposition. --- */
- bub::matrix<double,boost::numeric::ublas::column_major> QR2(freeRank,m2);
+ bub::matrix<double, boost::numeric::ublas::column_major> QR2(freeRank, m2);
Vector beta2(m2);
bub::vector<int> perm2(m2);
QR2.assign(Jt2);
- boost::numeric::bindings::lapack::geqp(QR2,perm2,beta2);
- const unsigned int & rank2 = rankDetermination(QR2);
+ boost::numeric::bindings::lapack::geqp(QR2, perm2, beta2);
+ const unsigned int &rank2 = rankDetermination(QR2);
/* --- Organize into Q and R. --- */
sotHouseholdMatrices Q2;
- Q2.push_back( QR2,beta2,rank2 );
- if( rank2<m2 ) // Rank deficiency
- {
- bub::matrix_range< bub::matrix<double,boost::numeric::ublas::column_major> >
- QR2sup( QR2,bub::range(0,rank2),bub::range(0,m2) );
- invGeneralizeCholeskiUpdate( QR2sup );
- }
- bub::matrix_range< bub::matrix<double,boost::numeric::ublas::column_major> >
- QR2sup( QR2,bub::range(0,rank2),bub::range(0,rank2) );
+ Q2.push_back(QR2, beta2, rank2);
+ if (rank2 < m2) // Rank deficiency
+ {
+ bub::matrix_range<
+ bub::matrix<double, boost::numeric::ublas::column_major>>
+ QR2sup(QR2, bub::range(0, rank2), bub::range(0, m2));
+ invGeneralizeCholeskiUpdate(QR2sup);
+ }
+ bub::matrix_range<bub::matrix<double, boost::numeric::ublas::column_major>>
+ QR2sup(QR2, bub::range(0, rank2), bub::range(0, rank2));
const MatrixTriSquare R2(QR2sup);
/* --- Compute the control law. --- */
@@ -813,160 +842,146 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
Vector u2(nJ);
/* et <- e - J2'u1 */
- Vector et2( m2 ); et2.assign( e2 );
- bub::axpy_prod( J2,-u,et2,false );
+ Vector et2(m2);
+ et2.assign(e2);
+ bub::axpy_prod(J2, -u, et2, false);
/* b <- Jt'et */
- bub::axpy_prod( Jt2,et2,b2,true );
+ bub::axpy_prod(Jt2, et2, b2, true);
/* b <- Q' Jt' et */
Q2.multiplyTransposeRight(b2);
/* Qe2 <- M' Jt' et */
- bub::vector_range<Vector> Qe2(b2,bub::range(0,rank2));
+ bub::vector_range<Vector> Qe2(b2, bub::range(0, rank2));
/* Qe2 <- R-2 M' Jt' et */
- bub::lu_substitute(R2,Qe2);
+ bub::lu_substitute(R2, Qe2);
/* Qe2 <- R'-1 R-1 M' Jt' et */
- bub::lu_substitute(Qe2,(const Matrix &)R2);
+ bub::lu_substitute(Qe2, (const Matrix &)R2);
/* b <- [ Qe2; 0 ] */
- bub::vector_range<Vector> Ne2(b2,bub::range(rank2,freeRank));
- Ne2.assign( bub::zero_vector<double>( freeRank-rank2-1 ) );
+ bub::vector_range<Vector> Ne2(b2, bub::range(rank2, freeRank));
+ Ne2.assign(bub::zero_vector<double>(freeRank - rank2 - 1));
/* b <- Q [ Qe2;0] = M R'-1 R-1 M' Jt' et */
Q2.multiplyRight(b2);
/* u2 <- N b = Q [0;I] b */
- Q.multiplyRangeRight( b2,u2,nJ-freeRank,0 );
+ Q.multiplyRangeRight(b2, u2, nJ - freeRank, 0);
/* Increment. */
- u+=u2;
+ u += u2;
freeRank -= rank2;
Q.push_back(Q2);
/* --- Traces --- */
- sotDEBUG(15) << "J"<<stage+1<<" = " << (MATLAB)J2 << std::endl;
- sotDEBUG(25) << "Jt"<<stage+1<<" = " << (MATLAB)Jt2 << std::endl;
- sotDEBUG(15) << "e"<<stage+1<<" = " << (MATLAB)e2 << std::endl;
- sotDEBUG(15) << "rank"<<stage+1<<" = " << rank2 << std::endl;
-
- sotDEBUG(25) << "QR"<<stage+1<<" = " << (MATLAB)QR2 << std::endl;
- sotDEBUG(45) << "Q"<<stage+1<<" = " << Q2;
- sotDEBUG(15) << "R"<<stage+1<<" = " << (MATLAB)R2 << std::endl;
- sotDEBUG(45) << "et"<<stage+1<<" = " << (MATLAB)et2 << std::endl;
-
- sotDEBUG(45) << "MRRMb"<<stage+1<<" = " << (MATLAB)b2 << std::endl;
- sotDEBUG(25) << "du"<<stage+1<<" = " << (MATLAB)u2 << std::endl;
-
- sotDEBUG(15) << "u"<<stage+1<<" = " << (MATLAB)u << std::endl;
- sotDEBUG(45) << "QA"<<stage+1<<" = " << Q << std::endl;
- sotDEBUG(25) << "freerank"<<stage+1<<" = " << freeRank << std::endl;
-
-
- }
+ sotDEBUG(15) << "J" << stage + 1 << " = " << (MATLAB)J2 << std::endl;
+ sotDEBUG(25) << "Jt" << stage + 1 << " = " << (MATLAB)Jt2 << std::endl;
+ sotDEBUG(15) << "e" << stage + 1 << " = " << (MATLAB)e2 << std::endl;
+ sotDEBUG(15) << "rank" << stage + 1 << " = " << rank2 << std::endl;
+
+ sotDEBUG(25) << "QR" << stage + 1 << " = " << (MATLAB)QR2 << std::endl;
+ sotDEBUG(45) << "Q" << stage + 1 << " = " << Q2;
+ sotDEBUG(15) << "R" << stage + 1 << " = " << (MATLAB)R2 << std::endl;
+ sotDEBUG(45) << "et" << stage + 1 << " = " << (MATLAB)et2 << std::endl;
+
+ sotDEBUG(45) << "MRRMb" << stage + 1 << " = " << (MATLAB)b2 << std::endl;
+ sotDEBUG(25) << "du" << stage + 1 << " = " << (MATLAB)u2 << std::endl;
+
+ sotDEBUG(15) << "u" << stage + 1 << " = " << (MATLAB)u << std::endl;
+ sotDEBUG(45) << "QA" << stage + 1 << " = " << Q << std::endl;
+ sotDEBUG(25) << "freerank" << stage + 1 << " = " << freeRank << std::endl;
+ }
control.accessToMotherLib() = u;
#ifdef VP_DEBUG
- for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter )
- {
- TaskAbstract & task = **iter;
- MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
- const dynamicgraph::Matrix &Jac = mem->JK;
- const dynamicgraph::Vector err;
- Sot::taskVectorToMlVector(task.taskSOUT.accessCopy(), err);
-
- dynamicgraph::Vector diffErr(err.size());
- diffErr = Jac*control;
- diffErr-=err;
- sotDEBUG(45)<<diffErr<<std::endl;
- }
+ for (StackType::iterator iter = stack.begin(); iter != stack.end(); ++iter) {
+ TaskAbstract &task = **iter;
+ MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(task.memoryInternal);
+ const dynamicgraph::Matrix &Jac = mem->JK;
+ const dynamicgraph::Vector err;
+ Sot::taskVectorToMlVector(task.taskSOUT.accessCopy(), err);
+
+ dynamicgraph::Vector diffErr(err.size());
+ diffErr = Jac * control;
+ diffErr -= err;
+ sotDEBUG(45) << diffErr << std::endl;
+ }
#endif // #ifdef VP_DEBUG
sotDEBUGOUT(15);
return control;
}
-
-
-dynamicgraph::Matrix& SotQr::
-computeConstraintProjector( dynamicgraph::Matrix& ProjK, const int& time )
-{
+dynamicgraph::Matrix &
+SotQr::computeConstraintProjector(dynamicgraph::Matrix &ProjK,
+ const int &time) {
sotDEBUGIN(15);
const dynamicgraph::Matrix *Jptr;
- if( 0==constraintList.size() )
- {
- const unsigned int FF_SIZE = ffJointIdLast-ffJointIdFirst;
- ProjK.resize( FF_SIZE,nbJoints-FF_SIZE ); ProjK.fill(.0);
- sotDEBUGOUT(15);
- return ProjK;
- }
- else if( 1==constraintList.size() )
- { Jptr = &(*constraintList.begin())->jacobianSOUT(time); }
- else
- {
- SOT_THROW ExceptionTask( ExceptionTask::EMPTY_LIST,
- "Not implemented yet." );
- }
+ if (0 == constraintList.size()) {
+ const unsigned int FF_SIZE = ffJointIdLast - ffJointIdFirst;
+ ProjK.resize(FF_SIZE, nbJoints - FF_SIZE);
+ ProjK.fill(.0);
+ sotDEBUGOUT(15);
+ return ProjK;
+ } else if (1 == constraintList.size()) {
+ Jptr = &(*constraintList.begin())->jacobianSOUT(time);
+ } else {
+ SOT_THROW ExceptionTask(ExceptionTask::EMPTY_LIST, "Not implemented yet.");
+ }
const dynamicgraph::Matrix &J = *Jptr;
- sotDEBUG(12) << "J = "<< J;
+ sotDEBUG(12) << "J = " << J;
const unsigned int nJc = J.cols();
- dynamicgraph::Matrix Jff( J.rows(),ffJointIdLast-ffJointIdFirst );
- dynamicgraph::Matrix Jc( J.rows(),nJc-ffJointIdLast+ffJointIdFirst );
-
- for( unsigned int i=0;i<J.rows();++i )
- {
- if( ffJointIdFirst )
- for( unsigned int j=0;j<ffJointIdFirst;++j )
- Jc(i,j)=J(i,j);
- if( ffJointIdLast<nJc )
- for( unsigned int j=ffJointIdLast;j<nJc;++j )
- Jc(i,j+ffJointIdFirst-ffJointIdLast)=J(i,j);
- for( unsigned int j=ffJointIdFirst;j<ffJointIdLast;++j )
- Jff( i,j-ffJointIdFirst )=J(i,j);
- }
- sotDEBUG(25) << "Jc = "<< Jc;
- sotDEBUG(25) << "Jff = "<< Jff;
+ dynamicgraph::Matrix Jff(J.rows(), ffJointIdLast - ffJointIdFirst);
+ dynamicgraph::Matrix Jc(J.rows(), nJc - ffJointIdLast + ffJointIdFirst);
+
+ for (unsigned int i = 0; i < J.rows(); ++i) {
+ if (ffJointIdFirst)
+ for (unsigned int j = 0; j < ffJointIdFirst; ++j)
+ Jc(i, j) = J(i, j);
+ if (ffJointIdLast < nJc)
+ for (unsigned int j = ffJointIdLast; j < nJc; ++j)
+ Jc(i, j + ffJointIdFirst - ffJointIdLast) = J(i, j);
+ for (unsigned int j = ffJointIdFirst; j < ffJointIdLast; ++j)
+ Jff(i, j - ffJointIdFirst) = J(i, j);
+ }
+ sotDEBUG(25) << "Jc = " << Jc;
+ sotDEBUG(25) << "Jff = " << Jff;
- dynamicgraph::Matrix Jffinv( Jff.cols(),Jff.rows() );
- Jff.pseudoInverse( Jffinv ); Jffinv *= -1;
+ dynamicgraph::Matrix Jffinv(Jff.cols(), Jff.rows());
+ Jff.pseudoInverse(Jffinv);
+ Jffinv *= -1;
- dynamicgraph::Matrix& Jffc = ProjK;
- Jffc.resize( Jffinv.rows(),Jc.cols() );
- Jffc = Jffinv*Jc;
- sotDEBUG(15) << "Jffc = "<< Jffc;
+ dynamicgraph::Matrix &Jffc = ProjK;
+ Jffc.resize(Jffinv.rows(), Jc.cols());
+ Jffc = Jffinv * Jc;
+ sotDEBUG(15) << "Jffc = " << Jffc;
sotDEBUGOUT(15);
return ProjK;
}
-
/* --------------------------------------------------------------------- */
/* --- DISPLAY --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void SotQr::
-display( std::ostream& os ) const
-{
-
- os << "+-----------------"<<std::endl<<"+ SOT "
- << std::endl<< "+-----------------"<<std::endl;
- for ( std::list<TaskAbstract*>::const_iterator it=this->stack.begin();
- this->stack.end()!=it;++it )
- {
- os << "| " << (*it)->getName() <<std::endl;
- }
- os<< "+-----------------"<<std::endl;
- if( taskGradient )
- {
- os << "| {Grad} " <<taskGradient->getName() << std::endl;
- os<< "+-----------------"<<std::endl;
- }
- for( ConstraintListType::const_iterator it = constraintList.begin();
- it!=constraintList.end();++it )
- { os<< "| K: "<< (*it)->getName() << endl; }
-
-
+void SotQr::display(std::ostream &os) const {
+
+ os << "+-----------------" << std::endl
+ << "+ SOT " << std::endl
+ << "+-----------------" << std::endl;
+ for (std::list<TaskAbstract *>::const_iterator it = this->stack.begin();
+ this->stack.end() != it; ++it) {
+ os << "| " << (*it)->getName() << std::endl;
+ }
+ os << "+-----------------" << std::endl;
+ if (taskGradient) {
+ os << "| {Grad} " << taskGradient->getName() << std::endl;
+ os << "+-----------------" << std::endl;
+ }
+ for (ConstraintListType::const_iterator it = constraintList.begin();
+ it != constraintList.end(); ++it) {
+ os << "| K: " << (*it)->getName() << endl;
+ }
}
-std::ostream&
-operator<< ( std::ostream& os,const SotQr& sot )
-{
+std::ostream &operator<<(std::ostream &os, const SotQr &sot) {
sot.display(os);
return os;
}
@@ -975,36 +990,33 @@ operator<< ( std::ostream& os,const SotQr& sot )
/* --- COMMAND --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-std::ostream& SotQr::
-writeGraph( std::ostream& os ) const
-{
+std::ostream &SotQr::writeGraph(std::ostream &os) const {
std::list<TaskAbstract *>::const_iterator iter;
- for( iter = stack.begin(); iter!=stack.end();++iter )
- {
- const TaskAbstract & task = **iter;
- std::list<TaskAbstract *>::const_iterator nextiter =iter;
- nextiter++;
-
- if (nextiter!=stack.end())
- {
- TaskAbstract & nexttask = **nextiter;
- os << "\t\t\t\"" << task.getName() << "\" -> \"" << nexttask.getName() << "\" [color=red]" << endl;
- }
-
+ for (iter = stack.begin(); iter != stack.end(); ++iter) {
+ const TaskAbstract &task = **iter;
+ std::list<TaskAbstract *>::const_iterator nextiter = iter;
+ nextiter++;
+
+ if (nextiter != stack.end()) {
+ TaskAbstract &nexttask = **nextiter;
+ os << "\t\t\t\"" << task.getName() << "\" -> \"" << nexttask.getName()
+ << "\" [color=red]" << endl;
}
+ }
- os << "\t\tsubgraph cluster_Tasks {" <<endl;
+ os << "\t\tsubgraph cluster_Tasks {" << endl;
os << "\t\t\tsubgraph \"cluster_" << getName() << "\" {" << std::endl;
- os << "\t\t\t\tcolor=lightsteelblue1; label=\"" << getName() <<"\"; style=filled;" << std::endl;
- for( iter = stack.begin(); iter!=stack.end();++iter )
- {
- const TaskAbstract & task = **iter;
- os << "\t\t\t\t\"" << task.getName()
- <<"\" [ label = \"" << task.getName() << "\" ," << std::endl
- <<"\t\t\t\t fontcolor = black, color = black, fillcolor = magenta, style=filled, shape=box ]" << std::endl;
-
- }
+ os << "\t\t\t\tcolor=lightsteelblue1; label=\"" << getName()
+ << "\"; style=filled;" << std::endl;
+ for (iter = stack.begin(); iter != stack.end(); ++iter) {
+ const TaskAbstract &task = **iter;
+ os << "\t\t\t\t\"" << task.getName() << "\" [ label = \"" << task.getName()
+ << "\" ," << std::endl
+ << "\t\t\t\t fontcolor = black, color = black, fillcolor = magenta, "
+ "style=filled, shape=box ]"
+ << std::endl;
+ }
os << "\t\t\t}" << std::endl;
- os << "\t\t\t}" <<endl;
+ os << "\t\t\t}" << endl;
return os;
}
diff --git a/src/sot/sot.cpp b/src/sot/sot.cpp
index a76ac0b7..fef5fd73 100644
--- a/src/sot/sot.cpp
+++ b/src/sot/sot.cpp
@@ -17,20 +17,20 @@
/* SOT */
#ifdef VP_DEBUG
-class sotSOT__INIT
-{
-public:sotSOT__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
+class sotSOT__INIT {
+public:
+ sotSOT__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
};
sotSOT__INIT sotSOT_initiator;
#endif //#ifdef VP_DEBUG
-#include <sot/core/sot.hh>
+#include <sot/core/factory.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>
-#include <sot/core/memory-task-sot.hh>
-#include <sot/core/matrix-svd.hh>
-#include <sot/core/matrix-geometry.hh>
-#include <sot/core/factory.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -42,236 +42,229 @@ using namespace dynamicgraph;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot,"SOT");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sot, "SOT");
const double Sot::INVERSION_THRESHOLD_DEFAULT = 1e-4;
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
-Sot::
-Sot( const std::string& name )
- :Entity(name)
- ,stack()
- ,constraintList()
- ,ffJointIdFirst( FF_JOINT_ID_DEFAULT )
- ,ffJointIdLast( FF_JOINT_ID_DEFAULT+6 )
-
- ,nbJoints( 0 )
- ,taskGradient(0)
- ,recomputeEachTime(true)
- ,q0SIN( NULL,"sotSOT("+name+")::input(double)::q0" )
- ,proj0SIN( NULL,"sotSOT("+name+")::input(double)::proj0" )
- ,inversionThresholdSIN( NULL,"sotSOT("+name+")::input(double)::damping" )
- ,constraintSOUT( boost::bind(&Sot::computeConstraintProjector,this,_1,_2),
- sotNOSIGNAL,
- "sotSOT("+name+")::output(matrix)::constraint" )
- ,controlSOUT( boost::bind(&Sot::computeControlLaw,this,_1,_2),
- constraintSOUT<<inversionThresholdSIN<<q0SIN<<proj0SIN,
- "sotSOT("+name+")::output(vector)::control" )
-{
+Sot::Sot(const std::string &name)
+ : Entity(name), stack(), constraintList(),
+ ffJointIdFirst(FF_JOINT_ID_DEFAULT),
+ ffJointIdLast(FF_JOINT_ID_DEFAULT + 6)
+
+ ,
+ nbJoints(0), taskGradient(0), recomputeEachTime(true),
+ q0SIN(NULL, "sotSOT(" + name + ")::input(double)::q0"),
+ proj0SIN(NULL, "sotSOT(" + name + ")::input(double)::proj0"),
+ inversionThresholdSIN(NULL,
+ "sotSOT(" + name + ")::input(double)::damping"),
+ constraintSOUT(
+ boost::bind(&Sot::computeConstraintProjector, this, _1, _2),
+ sotNOSIGNAL, "sotSOT(" + name + ")::output(matrix)::constraint"),
+ controlSOUT(boost::bind(&Sot::computeControlLaw, this, _1, _2),
+ constraintSOUT << inversionThresholdSIN << q0SIN << proj0SIN,
+ "sotSOT(" + name + ")::output(vector)::control") {
inversionThresholdSIN = INVERSION_THRESHOLD_DEFAULT;
- signalRegistration( inversionThresholdSIN<<controlSOUT<<constraintSOUT<<q0SIN<<proj0SIN );
+ signalRegistration(inversionThresholdSIN << controlSOUT << constraintSOUT
+ << q0SIN << proj0SIN);
// Commands
//
std::string docstring;
// addConstraint
- docstring =" \n"
- " AddConstraint\n"
- " \n"
- " Input:\n"
- " - a string: name of the constraint object\n"
- " \n";
+ docstring = " \n"
+ " AddConstraint\n"
+ " \n"
+ " Input:\n"
+ " - a string: name of the constraint object\n"
+ " \n";
addCommand("addConstraint",
- new command::classSot::AddConstraint(*this, docstring));
-
- 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";
+ new command::classSot::AddConstraint(*this, docstring));
+
+ 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";
addCommand("getSize",
- new dynamicgraph::command::Getter<Sot, const unsigned int&>
- (*this, &Sot::getNbDof, docstring));
-
- 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";
- 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";
- 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";
- addCommand("down",
- new command::classSot::Down(*this, docstring));
+ new dynamicgraph::command::Getter<Sot, const unsigned int &>(
+ *this, &Sot::getNbDof, docstring));
+
+ 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";
+ 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";
+ 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";
+ addCommand("down", new command::classSot::Down(*this, docstring));
// Display
- docstring =" \n"
- " display the list of tasks pushed inside the stack.\n"
- " \n";
- addCommand("display",
- new command::classSot::Display(*this, docstring));
-
+ 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";
- addCommand("clear",
- new command::classSot::Clear(*this, docstring));
+ 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";
- addCommand("list",
- new command::classSot::List(*this, docstring));
-
-
+ docstring = " \n"
+ " returns the list of tasks pushed inside the stack.\n"
+ " \n";
+ addCommand("list", new command::classSot::List(*this, docstring));
}
/* --------------------------------------------------------------------- */
/* --- STACK MANIPULATION --- */
/* --------------------------------------------------------------------- */
-void Sot::
-push( TaskAbstract& task )
-{
+void Sot::push(TaskAbstract &task) {
if (nbJoints == 0)
- throw std::logic_error ("Set joint size of "+ getClassName() + " \""+getName()+"\" first");
- stack.push_back( &task );
- controlSOUT.addDependency( task.taskSOUT );
- controlSOUT.addDependency( task.jacobianSOUT );
+ throw std::logic_error("Set joint size of " + getClassName() + " \"" +
+ getName() + "\" first");
+ stack.push_back(&task);
+ controlSOUT.addDependency(task.taskSOUT);
+ controlSOUT.addDependency(task.jacobianSOUT);
controlSOUT.setReady();
}
-TaskAbstract& Sot::
-pop( void )
-{
- TaskAbstract* res = stack.back();
+TaskAbstract &Sot::pop(void) {
+ TaskAbstract *res = stack.back();
stack.pop_back();
- controlSOUT.removeDependency( res->taskSOUT );
- controlSOUT.removeDependency( res->jacobianSOUT );
+ controlSOUT.removeDependency(res->taskSOUT);
+ controlSOUT.removeDependency(res->jacobianSOUT);
controlSOUT.setReady();
return *res;
}
-bool Sot::
-exist( const TaskAbstract& key )
-{
- std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { return true; }
+bool Sot::exist(const TaskAbstract &key) {
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ return true;
}
+ }
return false;
}
-void Sot::
-remove( const TaskAbstract& key )
-{
- bool find =false; std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void Sot::remove(const TaskAbstract &key) {
+ bool find = false;
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if(! find ){ return; }
+ }
+ if (!find) {
+ return;
+ }
- stack.erase( it );
- removeDependency( key );
+ stack.erase(it);
+ removeDependency(key);
}
-void Sot::
-removeDependency( const TaskAbstract& key )
-{
- controlSOUT.removeDependency( key.taskSOUT );
- controlSOUT.removeDependency( key.jacobianSOUT );
+void Sot::removeDependency(const TaskAbstract &key) {
+ controlSOUT.removeDependency(key.taskSOUT);
+ controlSOUT.removeDependency(key.jacobianSOUT);
controlSOUT.setReady();
}
-void Sot::
-up( const TaskAbstract& key )
-{
- bool find =false; std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void Sot::up(const TaskAbstract &key) {
+ bool find = false;
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if( stack.begin()==it ) { return; }
- if(! find ){ return; }
+ }
+ if (stack.begin() == it) {
+ return;
+ }
+ if (!find) {
+ return;
+ }
- std::list<TaskAbstract*>::iterator pos=it; pos--;
- TaskAbstract * task = *it;
- stack.erase( it );
- stack.insert( pos,task );
+ std::list<TaskAbstract *>::iterator pos = it;
+ pos--;
+ TaskAbstract *task = *it;
+ stack.erase(it);
+ stack.insert(pos, task);
controlSOUT.setReady();
}
-void Sot::
-down( const TaskAbstract& key )
-{
- bool find =false; std::list<TaskAbstract*>::iterator it;
- for ( it=stack.begin();stack.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void Sot::down(const TaskAbstract &key) {
+ bool find = false;
+ std::list<TaskAbstract *>::iterator it;
+ for (it = stack.begin(); stack.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if( stack.end()==it ) { return; }
- if(! find ){ return; }
+ }
+ if (stack.end() == it) {
+ return;
+ }
+ if (!find) {
+ return;
+ }
- std::list<TaskAbstract*>::iterator pos=it; pos++;
- TaskAbstract* task=*it;
- stack.erase( it );
- if( stack.end()==pos ){ stack.push_back(task); }
- else
- {
- pos++;
- stack.insert( pos,task );
- }
+ std::list<TaskAbstract *>::iterator pos = it;
+ pos++;
+ TaskAbstract *task = *it;
+ stack.erase(it);
+ if (stack.end() == pos) {
+ stack.push_back(task);
+ } else {
+ pos++;
+ stack.insert(pos, task);
+ }
controlSOUT.setReady();
}
-void Sot::
-clear( void )
-{
- for ( std::list<TaskAbstract*>::iterator it=stack.begin();stack.end()!=it;++it )
- {
- removeDependency( **it );
- }
+void Sot::clear(void) {
+ for (std::list<TaskAbstract *>::iterator it = stack.begin();
+ stack.end() != it; ++it) {
+ removeDependency(**it);
+ }
stack.clear();
controlSOUT.setReady();
}
@@ -280,49 +273,46 @@ clear( void )
/* --- CONSTRAINTS ----------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void Sot::
-addConstraint( Constraint& constraint )
-{
- constraintList.push_back( &constraint );
- constraintSOUT.addDependency( constraint.jacobianSOUT );
+void Sot::addConstraint(Constraint &constraint) {
+ constraintList.push_back(&constraint);
+ constraintSOUT.addDependency(constraint.jacobianSOUT);
}
-void Sot::
-removeConstraint( const Constraint& key )
-{
- bool find =false; ConstraintListType::iterator it;
- for ( it=constraintList.begin();constraintList.end()!=it;++it )
- {
- if( *it == &key ) { find=true; break; }
+void Sot::removeConstraint(const Constraint &key) {
+ bool find = false;
+ ConstraintListType::iterator it;
+ for (it = constraintList.begin(); constraintList.end() != it; ++it) {
+ if (*it == &key) {
+ find = true;
+ break;
}
- if(! find ){ return; }
+ }
+ if (!find) {
+ return;
+ }
- constraintList.erase( it );
+ constraintList.erase(it);
- constraintSOUT.removeDependency( key.jacobianSOUT );
+ constraintSOUT.removeDependency(key.jacobianSOUT);
}
-void Sot::
-clearConstraint( void )
-{
- for ( ConstraintListType::iterator it=constraintList.begin();
- constraintList.end()!=it;++it )
- {
- constraintSOUT.removeDependency( (*it)->jacobianSOUT );
- }
+void Sot::clearConstraint(void) {
+ for (ConstraintListType::iterator it = constraintList.begin();
+ constraintList.end() != it; ++it) {
+ constraintSOUT.removeDependency((*it)->jacobianSOUT);
+ }
constraintList.clear();
}
-void Sot::
-defineFreeFloatingJoints( const unsigned int& first,const unsigned int& last )
-{
- ffJointIdFirst = first ;
- if( last>0 ) ffJointIdLast=last ;
- else ffJointIdLast=ffJointIdFirst+6;
+void Sot::defineFreeFloatingJoints(const unsigned int &first,
+ const unsigned int &last) {
+ ffJointIdFirst = first;
+ if (last > 0)
+ ffJointIdLast = last;
+ else
+ ffJointIdLast = ffJointIdFirst + 6;
}
-void Sot::
-defineNbDof( const unsigned int& nbDof )
-{
+void Sot::defineNbDof(const unsigned int &nbDof) {
nbJoints = nbDof;
constraintSOUT.setReady();
controlSOUT.setReady();
@@ -332,11 +322,10 @@ defineNbDof( const unsigned int& nbDof )
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-dynamicgraph::Matrix & Sot::
-computeJacobianConstrained( const dynamicgraph::Matrix& Jac,
- const dynamicgraph::Matrix& K,
- dynamicgraph::Matrix& JK)
-{
+dynamicgraph::Matrix &
+Sot::computeJacobianConstrained(const dynamicgraph::Matrix &Jac,
+ const dynamicgraph::Matrix &K,
+ dynamicgraph::Matrix &JK) {
const Matrix::Index nJ = Jac.rows();
const Matrix::Index mJ = K.cols();
const Matrix::Index nbConstraints = Jac.cols() - mJ;
@@ -346,67 +335,54 @@ computeJacobianConstrained( const dynamicgraph::Matrix& Jac,
return JK;
}
JK.resize(nJ, mJ);
- JK.noalias() = Jac.leftCols (nbConstraints) * K;
- JK.noalias() += Jac.rightCols (Jac.cols() - nbConstraints);
+ JK.noalias() = Jac.leftCols(nbConstraints) * K;
+ JK.noalias() += Jac.rightCols(Jac.cols() - nbConstraints);
return JK;
}
-
-dynamicgraph::Matrix & Sot::
-computeJacobianConstrained( const TaskAbstract& task,
- const dynamicgraph::Matrix& K )
-{
- const dynamicgraph::Matrix &Jac = task.jacobianSOUT.accessCopy ();
- MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
- if( NULL==mem ) throw; // TODO
+dynamicgraph::Matrix &
+Sot::computeJacobianConstrained(const TaskAbstract &task,
+ const dynamicgraph::Matrix &K) {
+ const dynamicgraph::Matrix &Jac = task.jacobianSOUT.accessCopy();
+ MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(task.memoryInternal);
+ if (NULL == mem)
+ throw; // TODO
dynamicgraph::Matrix &JK = mem->JK;
- return computeJacobianConstrained(Jac,K,JK);
+ return computeJacobianConstrained(Jac, K, JK);
}
-static void computeJacobianActivated( Task* taskSpec,
- dynamicgraph::Matrix& Jt,
- const int& iterTime )
-{
- if( NULL!=taskSpec )
- {
- const Flags& controlSelec = taskSpec->controlSelectionSIN( iterTime );
- sotDEBUG(25) << "Control selection = " << controlSelec <<endl;
- if( controlSelec )
- {
- if(!controlSelec)
- {
- sotDEBUG(15) << "Control selection."<<endl;
- for( int i=0;i<Jt.cols();++i )
- {
- if(! controlSelec(i) )
- { Jt.col (i).setZero(); }
- }
- }
- else
- {
- sotDEBUG(15) << "S is equal to Id."<<endl;
- }
- }
- else
- {
- sotDEBUG(15) << "Task not activated."<<endl;
- Jt *= 0;
- }
+static void computeJacobianActivated(Task *taskSpec, dynamicgraph::Matrix &Jt,
+ const int &iterTime) {
+ if (NULL != taskSpec) {
+ const Flags &controlSelec = taskSpec->controlSelectionSIN(iterTime);
+ sotDEBUG(25) << "Control selection = " << controlSelec << endl;
+ if (controlSelec) {
+ if (!controlSelec) {
+ sotDEBUG(15) << "Control selection." << endl;
+ for (int i = 0; i < Jt.cols(); ++i) {
+ if (!controlSelec(i)) {
+ Jt.col(i).setZero();
+ }
+ }
+ } else {
+ sotDEBUG(15) << "S is equal to Id." << endl;
+ }
+ } else {
+ sotDEBUG(15) << "Task not activated." << endl;
+ Jt *= 0;
}
- else { /* No selection specification: nothing to do. */ }
+ } else { /* No selection specification: nothing to do. */
+ }
}
-
/* --------------------------------------------------------------------- */
/* --- CONTROL --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
//#define WITH_CHRONO
-
-#ifdef WITH_CHRONO
+#ifdef WITH_CHRONO
#ifndef WIN32
#include <sys/time.h>
#else /*WIN32*/
@@ -414,55 +390,64 @@ static void computeJacobianActivated( Task* taskSpec,
#endif /*WIN32*/
#endif /*WITH_CHRONO*/
-
-#ifdef WITH_CHRONO
-# define sotINIT_CHRONO1 struct timeval t0,t1; double dt
-# define sotSTART_CHRONO1 gettimeofday(&t0,NULL)
-# define sotCHRONO1 \
- gettimeofday(&t1,NULL); \
- dt = ( (double)(t1.tv_sec-t0.tv_sec) * 1000.* 1000. \
- + (double)(t1.tv_usec-t0.tv_usec) ); \
- sotDEBUG(1) << "dt: "<< dt / 1000. << std::endl
-# define sotINITPARTCOUNTERS struct timeval tpart0
-# define sotSTARTPARTCOUNTERS gettimeofday(&tpart0,NULL)
-# define sotCOUNTER(nbc1,nbc2) \
- gettimeofday(&tpart##nbc2,NULL); \
- dt##nbc2 += ( (double)(tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000.* 1000. \
- + (double)(tpart##nbc2.tv_usec-tpart##nbc1.tv_usec) )
-# define sotINITCOUNTER(nbc1) \
- struct timeval tpart##nbc1; double dt##nbc1=0;
-# define sotPRINTCOUNTER(nbc1) sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
+#ifdef WITH_CHRONO
+#define sotINIT_CHRONO1 \
+ struct timeval t0, t1; \
+ double dt
+#define sotSTART_CHRONO1 gettimeofday(&t0, NULL)
+#define sotCHRONO1 \
+ gettimeofday(&t1, NULL); \
+ dt = ((double)(t1.tv_sec - t0.tv_sec) * 1000. * 1000. + \
+ (double)(t1.tv_usec - t0.tv_usec)); \
+ sotDEBUG(1) << "dt: " << dt / 1000. << std::endl
+#define sotINITPARTCOUNTERS struct timeval tpart0
+#define sotSTARTPARTCOUNTERS gettimeofday(&tpart0, NULL)
+#define sotCOUNTER(nbc1, nbc2) \
+ gettimeofday(&tpart##nbc2, NULL); \
+ dt##nbc2 += \
+ ((double)(tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1000. * 1000. + \
+ (double)(tpart##nbc2.tv_usec - tpart##nbc1.tv_usec))
+#define sotINITCOUNTER(nbc1) \
+ struct timeval tpart##nbc1; \
+ double dt##nbc1 = 0;
+#define sotPRINTCOUNTER(nbc1) \
+ sotDEBUG(1) << "dt" << nbc1 << " = " << dt##nbc1 << std::endl
#else // #ifdef WITH_CHRONO
-# define sotINIT_CHRONO1
-# define sotSTART_CHRONO1
-# define sotCHRONO1
-# define sotINITPARTCOUNTERS
-# define sotSTARTPARTCOUNTERS
-# define sotCOUNTER(nbc1,nbc2)
-# define sotINITCOUNTER(nbc1)
-# define sotPRINTCOUNTER(nbc1)
+#define sotINIT_CHRONO1
+#define sotSTART_CHRONO1
+#define sotCHRONO1
+#define sotINITPARTCOUNTERS
+#define sotSTARTPARTCOUNTERS
+#define sotCOUNTER(nbc1, nbc2)
+#define sotINITCOUNTER(nbc1)
+#define sotPRINTCOUNTER(nbc1)
#endif // #ifdef WITH_CHRONO
-void Sot::
-taskVectorToMlVector( const VectorMultiBound& taskVector, Vector& res )
-{
+void Sot::taskVectorToMlVector(const VectorMultiBound &taskVector,
+ Vector &res) {
res.resize(taskVector.size());
- unsigned int i=0;
+ unsigned int i = 0;
- for( VectorMultiBound::const_iterator iter=taskVector.begin();
- iter!=taskVector.end();++iter,++i ) {
- res(i)=iter->getSingleBound();
+ for (VectorMultiBound::const_iterator iter = taskVector.begin();
+ iter != taskVector.end(); ++iter, ++i) {
+ res(i) = iter->getSingleBound();
}
}
-dynamicgraph::Vector& Sot::
-computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
-{
+dynamicgraph::Vector &Sot::computeControlLaw(dynamicgraph::Vector &control,
+ const int &iterTime) {
sotDEBUGIN(15);
- sotINIT_CHRONO1; sotINITPARTCOUNTERS;
- sotINITCOUNTER(1); sotINITCOUNTER(2);sotINITCOUNTER(3);sotINITCOUNTER(4);
- sotINITCOUNTER(5); sotINITCOUNTER(6);sotINITCOUNTER(7);sotINITCOUNTER(8);
+ sotINIT_CHRONO1;
+ sotINITPARTCOUNTERS;
+ sotINITCOUNTER(1);
+ sotINITCOUNTER(2);
+ sotINITCOUNTER(3);
+ sotINITCOUNTER(4);
+ sotINITCOUNTER(5);
+ sotINITCOUNTER(6);
+ sotINITCOUNTER(7);
+ sotINITCOUNTER(8);
sotSTART_CHRONO1;
sotSTARTPARTCOUNTERS;
@@ -472,337 +457,341 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
const Matrix::Index mJ = K.cols(); // number dofs - number constraints
if (q0SIN.isPlugged()) {
- control = q0SIN( iterTime );
+ control = q0SIN(iterTime);
if (control.size() != mJ) {
std::ostringstream oss;
- oss << "SOT(" << getName() << "): q0SIN value length is " << control.size()
- << "while the expected lenth is " << mJ;
- throw std::length_error (oss.str());
+ oss << "SOT(" << getName() << "): q0SIN value length is "
+ << control.size() << "while the expected lenth is " << mJ;
+ throw std::length_error(oss.str());
}
} else {
if (stack.size() == 0) {
std::ostringstream oss;
- oss << "SOT(" << getName() << ") contains no task and q0SIN is not plugged.";
- throw std::logic_error (oss.str());
+ oss << "SOT(" << getName()
+ << ") contains no task and q0SIN is not plugged.";
+ throw std::logic_error(oss.str());
}
- control = Vector::Zero (mJ);
- sotDEBUG(25) << "No initial velocity." <<endl;
+ control = Vector::Zero(mJ);
+ sotDEBUG(25) << "No initial velocity." << endl;
}
- sotDEBUGF(5, " --- Time %d -------------------", iterTime );
+ sotDEBUGF(5, " --- Time %d -------------------", iterTime);
unsigned int iterTask = 0;
- const Matrix* PrevProj = NULL;
+ const Matrix *PrevProj = NULL;
// Get initial projector if any.
if (proj0SIN.isPlugged()) {
- const Matrix& initialProjector = proj0SIN.access (iterTime);
+ const Matrix &initialProjector = proj0SIN.access(iterTime);
PrevProj = &initialProjector;
}
- for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter )
- {
- sotDEBUGF(5,"Rank %d.",iterTask);
- TaskAbstract & task = **iter;
- sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
- const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
- sotCOUNTER(0,1); // Direct Dynamic
-
- unsigned int rankJ;
- const Matrix::Index nJ = Jac.rows(); // number dofs
-
- /* Init memory. */
- MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
- if( NULL==mem )
- {
- if( NULL!=task.memoryInternal ) delete task.memoryInternal;
- mem = new MemoryTaskSOT( task.getName()+"_memSOT",nJ,mJ );
- task.memoryInternal = mem;
- }
-
- Matrix &Jp = mem->Jp;
- Matrix &JK = mem->JK;
- Matrix &Jt = mem->Jt;
- Matrix &Proj = mem->Proj;
- MemoryTaskSOT::SVD_t& svd = mem->svd;
-
- taskVectorToMlVector(task.taskSOUT(iterTime), mem->err);
- const dynamicgraph::Vector &err = mem->err;
-
- Jp.resize( mJ,nJ );
- Jt.resize( nJ,mJ );
- JK.resize( nJ,mJ );
-
- if( (recomputeEachTime)
- ||(task.jacobianSOUT.getTime()>mem->jacobianInvSINOUT.getTime())
- ||(mem->jacobianInvSINOUT.accessCopy().rows()!=mJ)
- ||(mem->jacobianInvSINOUT.accessCopy().cols()!=nJ)
- ||(task.jacobianSOUT.getTime()>mem->jacobianConstrainedSINOUT.getTime())
- ||(task.jacobianSOUT.getTime()>mem->rankSINOUT.getTime())
- ||(task.jacobianSOUT.getTime()>mem->singularBaseImageSINOUT.getTime()) )
- {
- sotDEBUG(2) <<"Recompute inverse."<<endl;
-
- /* --- FIRST ALLOCS --- */
- sotDEBUG(1) << "Size = "
- << std::min(nJ, mJ) + mem->Jff.cols()*mem->Jff.rows()
- + mem->Jact.cols()*mem->Jact.rows() << std::endl;
-
- sotDEBUG(1) << std::endl;
- sotDEBUG(1) << "nJ=" << nJ << " " << "Jac.cols()=" << Jac.cols()
- <<" "<< "mJ=" << mJ<<std::endl;
- mem->Jff.resize( nJ,Jac.cols()-mJ ); // number dofs, number constraints
- sotDEBUG(1) << std::endl;
- mem->Jact.resize( nJ,mJ );
- sotDEBUG(1) << std::endl;
- sotDEBUG(1) << "Size = "
- << std::min(nJ, mJ) + mem->Jff.cols()*mem->Jff.rows()
- + mem->Jact.cols()*mem->Jact.rows() << std::endl;
-
- /***/sotCOUNTER(1,2); // first allocs
-
- /* --- COMPUTE JK --- */
- computeJacobianConstrained( task,K );
- /***/sotCOUNTER(2,3); // compute JK
-
- /* --- COMPUTE S --- */
- computeJacobianActivated( dynamic_cast<Task*>( &task ),JK,iterTime );
- /***/sotCOUNTER(3,4); // compute JK*S
-
- /* --- COMPUTE Jt --- */
- if( PrevProj!=NULL ) Jt.noalias() = JK*(*PrevProj); else { Jt = JK; }
- /***/sotCOUNTER(4,5); // compute Jt
-
- /* --- PINV --- */
- svd.compute (Jt);
- Eigen::dampedInverse (svd, Jp, th);
- /***/sotCOUNTER(5,6); // PINV
- sotDEBUG(20) << "V after dampedInverse." << svd.matrixV() <<endl;
- /* --- RANK --- */
- {
- rankJ = 0;
- while ( rankJ < svd.singularValues().size()
- && th < svd.singularValues()[rankJ])
- { ++rankJ; }
- }
-
- sotDEBUG(45) << "control"<<iterTask<<" = "<<control<<endl;
- sotDEBUG(25) << "J"<<iterTask<<" = "<<Jac<<endl;
- sotDEBUG(25) << "JK"<<iterTask<<" = "<<JK<<endl;
- sotDEBUG(25) << "Jt"<<iterTask<<" = "<<Jt<<endl;
- sotDEBUG(15) << "Jp"<<iterTask<<" = "<<Jp<<endl;
- sotDEBUG(15) << "e"<<iterTask<<" = "<<err<<endl;
- sotDEBUG(45) << "JJp"<<iterTask<<" = "<< JK*Jp <<endl;
- //sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
- sotDEBUG(45) << "S"<<iterTask<<" = "<< svd.singularValues()<<endl;
- sotDEBUG(45) << "V"<<iterTask<<" = "<< svd.matrixV()<<endl;
- sotDEBUG(45) << "U"<<iterTask<<" = "<< svd.matrixU()<<endl;
-
- mem->jacobianInvSINOUT = Jp;
- mem->jacobianInvSINOUT.setTime( iterTime );
- mem->jacobianConstrainedSINOUT = JK;
- mem->jacobianConstrainedSINOUT.setTime( iterTime );
- mem->jacobianProjectedSINOUT = Jt;
- mem->jacobianProjectedSINOUT.setTime( iterTime );
- mem->singularBaseImageSINOUT = svd.matrixV().leftCols(rankJ);
- mem->singularBaseImageSINOUT.setTime( iterTime );
- mem->rankSINOUT = rankJ;
- mem->rankSINOUT.setTime( iterTime );
-
- sotDEBUG(25)<<"Inverse recomputed."<<endl;
- } else {
- sotDEBUG(2)<<"Inverse not recomputed."<<endl;
- rankJ = mem->rankSINOUT.accessCopy();
- Jp = mem->jacobianInvSINOUT.accessCopy();
- JK = mem->jacobianConstrainedSINOUT.accessCopy ();
- Jt = mem->jacobianProjectedSINOUT.accessCopy ();
- }
- /***/sotCOUNTER(6,7); // TRACE
-
- /* --- COMPUTE QDOT AND P --- */
- /*DEBUG: normally, the first iter (ie the test below)
- * is the same than the other, starting with control_0 = q0SIN. */
- if( PrevProj == NULL ) control.noalias() += Jp*err;
- else control += *PrevProj * (Jp*(err - JK*control));
- /***/sotCOUNTER(7,8); // QDOT
-
- /* --- OPTIMAL FORM: To debug. --- */
- if( PrevProj == NULL ) {
- Proj.noalias() = svd.matrixV().rightCols(svd.matrixV().cols()-rankJ);
- } else {
- Proj.noalias() = *PrevProj * svd.matrixV().rightCols(svd.matrixV().cols()-rankJ);
- }
-
- /* --- OLIVIER START --- */
- // Update by Joseph Mirabel to match Eigen API
-
- sotDEBUG(2) << "Proj non optimal (rankJ= " <<rankJ
- << ", iterTask =" << iterTask
- << ")";
- sotDEBUG(20) << "V = " << svd.matrixV();
- sotDEBUG(20) << "Jt = " << Jt;
- sotDEBUG(20) << "JpxJt = " << Jp*Jt;
- sotDEBUG(25) << "Proj-Jp*Jt"<<iterTask<<" = "<< (Proj-Jp*Jt) <<endl;
-
- /* --- OLIVIER END --- */
-
- sotDEBUG(15) << "q"<<iterTask<<" = "<<control<<std::endl;
- sotDEBUG(25) << "P"<<iterTask<<" = "<< Proj <<endl;
- iterTask++;
- PrevProj = &Proj;
-
- sotPRINTCOUNTER(1); sotPRINTCOUNTER(2); sotPRINTCOUNTER(3);
- sotPRINTCOUNTER(4); sotPRINTCOUNTER(5); sotPRINTCOUNTER(6);
- sotPRINTCOUNTER(7); sotPRINTCOUNTER(8);
-
+ for (StackType::iterator iter = stack.begin(); iter != stack.end(); ++iter) {
+ sotDEBUGF(5, "Rank %d.", iterTask);
+ TaskAbstract &task = **iter;
+ sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
+ const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
+ sotCOUNTER(0, 1); // Direct Dynamic
+
+ unsigned int rankJ;
+ const Matrix::Index nJ = Jac.rows(); // number dofs
+
+ /* Init memory. */
+ MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(task.memoryInternal);
+ if (NULL == mem) {
+ if (NULL != task.memoryInternal)
+ delete task.memoryInternal;
+ mem = new MemoryTaskSOT(task.getName() + "_memSOT", nJ, mJ);
+ task.memoryInternal = mem;
}
- sotCHRONO1;
+ Matrix &Jp = mem->Jp;
+ Matrix &JK = mem->JK;
+ Matrix &Jt = mem->Jt;
+ Matrix &Proj = mem->Proj;
+ MemoryTaskSOT::SVD_t &svd = mem->svd;
+
+ taskVectorToMlVector(task.taskSOUT(iterTime), mem->err);
+ const dynamicgraph::Vector &err = mem->err;
+
+ Jp.resize(mJ, nJ);
+ Jt.resize(nJ, mJ);
+ JK.resize(nJ, mJ);
+
+ if ((recomputeEachTime) ||
+ (task.jacobianSOUT.getTime() > mem->jacobianInvSINOUT.getTime()) ||
+ (mem->jacobianInvSINOUT.accessCopy().rows() != mJ) ||
+ (mem->jacobianInvSINOUT.accessCopy().cols() != nJ) ||
+ (task.jacobianSOUT.getTime() >
+ mem->jacobianConstrainedSINOUT.getTime()) ||
+ (task.jacobianSOUT.getTime() > mem->rankSINOUT.getTime()) ||
+ (task.jacobianSOUT.getTime() >
+ mem->singularBaseImageSINOUT.getTime())) {
+ sotDEBUG(2) << "Recompute inverse." << endl;
+
+ /* --- FIRST ALLOCS --- */
+ sotDEBUG(1) << "Size = "
+ << std::min(nJ, mJ) + mem->Jff.cols() * mem->Jff.rows() +
+ mem->Jact.cols() * mem->Jact.rows()
+ << std::endl;
+
+ sotDEBUG(1) << std::endl;
+ sotDEBUG(1) << "nJ=" << nJ << " "
+ << "Jac.cols()=" << Jac.cols() << " "
+ << "mJ=" << mJ << std::endl;
+ mem->Jff.resize(nJ, Jac.cols() - mJ); // number dofs, number constraints
+ sotDEBUG(1) << std::endl;
+ mem->Jact.resize(nJ, mJ);
+ sotDEBUG(1) << std::endl;
+ sotDEBUG(1) << "Size = "
+ << std::min(nJ, mJ) + mem->Jff.cols() * mem->Jff.rows() +
+ mem->Jact.cols() * mem->Jact.rows()
+ << std::endl;
+
+ /***/ sotCOUNTER(1, 2); // first allocs
- if( 0!=taskGradient )
- {
- const dynamicgraph::Matrix & Jac = taskGradient->jacobianSOUT.access(iterTime);
+ /* --- COMPUTE JK --- */
+ computeJacobianConstrained(task, K);
+ /***/ sotCOUNTER(2, 3); // compute JK
- const Matrix::Index nJ = Jac.rows();
+ /* --- COMPUTE S --- */
+ computeJacobianActivated(dynamic_cast<Task *>(&task), JK, iterTime);
+ /***/ sotCOUNTER(3, 4); // compute JK*S
- MemoryTaskSOT * mem
- = dynamic_cast<MemoryTaskSOT *>( taskGradient->memoryInternal );
- if( NULL==mem )
- {
- if( NULL!=taskGradient->memoryInternal )
- { delete taskGradient->memoryInternal; }
- mem = new MemoryTaskSOT( taskGradient->getName()+"_memSOT",nJ,mJ );
- taskGradient->memoryInternal = mem;
+ /* --- COMPUTE Jt --- */
+ if (PrevProj != NULL)
+ Jt.noalias() = JK * (*PrevProj);
+ else {
+ Jt = JK;
+ }
+ /***/ sotCOUNTER(4, 5); // compute Jt
+
+ /* --- PINV --- */
+ svd.compute(Jt);
+ Eigen::dampedInverse(svd, Jp, th);
+ /***/ sotCOUNTER(5, 6); // PINV
+ sotDEBUG(20) << "V after dampedInverse." << svd.matrixV() << endl;
+ /* --- RANK --- */
+ {
+ rankJ = 0;
+ while (rankJ < svd.singularValues().size() &&
+ th < svd.singularValues()[rankJ]) {
+ ++rankJ;
}
+ }
- taskVectorToMlVector(taskGradient->taskSOUT.access(iterTime), mem->err);
- const dynamicgraph::Vector& err = mem->err;
-
- sotDEBUG(45) << "K = " << K <<endl;
- sotDEBUG(45) << "Jff = " << Jac <<endl;
-
- /* --- MEMORY INIT --- */
- dynamicgraph::Matrix &Jp = mem->Jp;
- dynamicgraph::Matrix &PJp = mem->PJp;
- dynamicgraph::Matrix &Jt = mem->Jt;
- MemoryTaskSOT::SVD_t& svd = mem->svd;
-
- mem->JK.resize( nJ,mJ );
- mem->Jt.resize( nJ,mJ );
- mem->Jff.resize( nJ,Jac.cols()-mJ );
- mem->Jact.resize( nJ,mJ );
- Jp.resize( mJ,nJ );
- PJp.resize( nJ,mJ );
+ sotDEBUG(45) << "control" << iterTask << " = " << control << endl;
+ sotDEBUG(25) << "J" << iterTask << " = " << Jac << endl;
+ sotDEBUG(25) << "JK" << iterTask << " = " << JK << endl;
+ sotDEBUG(25) << "Jt" << iterTask << " = " << Jt << endl;
+ sotDEBUG(15) << "Jp" << iterTask << " = " << Jp << endl;
+ sotDEBUG(15) << "e" << iterTask << " = " << err << endl;
+ sotDEBUG(45) << "JJp" << iterTask << " = " << JK * Jp << endl;
+ // sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
+ sotDEBUG(45) << "S" << iterTask << " = " << svd.singularValues() << endl;
+ sotDEBUG(45) << "V" << iterTask << " = " << svd.matrixV() << endl;
+ sotDEBUG(45) << "U" << iterTask << " = " << svd.matrixU() << endl;
+
+ mem->jacobianInvSINOUT = Jp;
+ mem->jacobianInvSINOUT.setTime(iterTime);
+ mem->jacobianConstrainedSINOUT = JK;
+ mem->jacobianConstrainedSINOUT.setTime(iterTime);
+ mem->jacobianProjectedSINOUT = Jt;
+ mem->jacobianProjectedSINOUT.setTime(iterTime);
+ mem->singularBaseImageSINOUT = svd.matrixV().leftCols(rankJ);
+ mem->singularBaseImageSINOUT.setTime(iterTime);
+ mem->rankSINOUT = rankJ;
+ mem->rankSINOUT.setTime(iterTime);
+
+ sotDEBUG(25) << "Inverse recomputed." << endl;
+ } else {
+ sotDEBUG(2) << "Inverse not recomputed." << endl;
+ rankJ = mem->rankSINOUT.accessCopy();
+ Jp = mem->jacobianInvSINOUT.accessCopy();
+ JK = mem->jacobianConstrainedSINOUT.accessCopy();
+ Jt = mem->jacobianProjectedSINOUT.accessCopy();
+ }
+ /***/ sotCOUNTER(6, 7); // TRACE
+
+ /* --- COMPUTE QDOT AND P --- */
+ /*DEBUG: normally, the first iter (ie the test below)
+ * is the same than the other, starting with control_0 = q0SIN. */
+ if (PrevProj == NULL)
+ control.noalias() += Jp * err;
+ else
+ control += *PrevProj * (Jp * (err - JK * control));
+ /***/ sotCOUNTER(7, 8); // QDOT
+
+ /* --- OPTIMAL FORM: To debug. --- */
+ if (PrevProj == NULL) {
+ Proj.noalias() = svd.matrixV().rightCols(svd.matrixV().cols() - rankJ);
+ } else {
+ Proj.noalias() =
+ *PrevProj * svd.matrixV().rightCols(svd.matrixV().cols() - rankJ);
+ }
- /* --- COMPUTE JK --- */
- dynamicgraph::Matrix &JK = computeJacobianConstrained( *taskGradient,K);
+ /* --- OLIVIER START --- */
+ // Update by Joseph Mirabel to match Eigen API
+
+ sotDEBUG(2) << "Proj non optimal (rankJ= " << rankJ
+ << ", iterTask =" << iterTask << ")";
+ sotDEBUG(20) << "V = " << svd.matrixV();
+ sotDEBUG(20) << "Jt = " << Jt;
+ sotDEBUG(20) << "JpxJt = " << Jp * Jt;
+ sotDEBUG(25) << "Proj-Jp*Jt" << iterTask << " = " << (Proj - Jp * Jt)
+ << endl;
+
+ /* --- OLIVIER END --- */
+
+ sotDEBUG(15) << "q" << iterTask << " = " << control << std::endl;
+ sotDEBUG(25) << "P" << iterTask << " = " << Proj << endl;
+ iterTask++;
+ PrevProj = &Proj;
+
+ sotPRINTCOUNTER(1);
+ sotPRINTCOUNTER(2);
+ sotPRINTCOUNTER(3);
+ sotPRINTCOUNTER(4);
+ sotPRINTCOUNTER(5);
+ sotPRINTCOUNTER(6);
+ sotPRINTCOUNTER(7);
+ sotPRINTCOUNTER(8);
+ }
- /* --- COMPUTE Jinv --- */
- sotDEBUG(35) << "grad = " << err <<endl;
- sotDEBUG(35) << "Jgrad = " << JK <<endl;
+ sotCHRONO1;
- // Use optimized-memory Jt to do the p-inverse.
- Jt=JK;
- svd.compute (Jt);
- // TODO the two next lines could be replaced by
- Eigen::dampedInverse( svd, Jp,th );
- if (PrevProj != NULL)
- PJp.noalias() = (*PrevProj)*Jp;
- else
- PJp.noalias() = Jp;
+ if (0 != taskGradient) {
+ const dynamicgraph::Matrix &Jac =
+ taskGradient->jacobianSOUT.access(iterTime);
- /* --- COMPUTE ERR --- */
- const dynamicgraph::Vector& Herr( err );
+ const Matrix::Index nJ = Jac.rows();
- /* --- COMPUTE CONTROL --- */
- control.noalias() += PJp*Herr;
+ MemoryTaskSOT *mem =
+ dynamic_cast<MemoryTaskSOT *>(taskGradient->memoryInternal);
+ if (NULL == mem) {
+ if (NULL != taskGradient->memoryInternal) {
+ delete taskGradient->memoryInternal;
+ }
+ mem = new MemoryTaskSOT(taskGradient->getName() + "_memSOT", nJ, mJ);
+ taskGradient->memoryInternal = mem;
+ }
- /* --- TRACE --- */
- sotDEBUG(45) << "Pgrad = " << (PJp*Herr) <<endl;
- if (PrevProj != NULL) { sotDEBUG(45) << "P = " << *PrevProj <<endl; }
- sotDEBUG(45) << "Jp = " << Jp <<endl;
- sotDEBUG(45) << "PJp = " << PJp <<endl;
+ taskVectorToMlVector(taskGradient->taskSOUT.access(iterTime), mem->err);
+ const dynamicgraph::Vector &err = mem->err;
+
+ sotDEBUG(45) << "K = " << K << endl;
+ sotDEBUG(45) << "Jff = " << Jac << endl;
+
+ /* --- MEMORY INIT --- */
+ dynamicgraph::Matrix &Jp = mem->Jp;
+ dynamicgraph::Matrix &PJp = mem->PJp;
+ dynamicgraph::Matrix &Jt = mem->Jt;
+ MemoryTaskSOT::SVD_t &svd = mem->svd;
+
+ mem->JK.resize(nJ, mJ);
+ mem->Jt.resize(nJ, mJ);
+ mem->Jff.resize(nJ, Jac.cols() - mJ);
+ mem->Jact.resize(nJ, mJ);
+ Jp.resize(mJ, nJ);
+ PJp.resize(nJ, mJ);
+
+ /* --- COMPUTE JK --- */
+ dynamicgraph::Matrix &JK = computeJacobianConstrained(*taskGradient, K);
+
+ /* --- COMPUTE Jinv --- */
+ sotDEBUG(35) << "grad = " << err << endl;
+ sotDEBUG(35) << "Jgrad = " << JK << endl;
+
+ // Use optimized-memory Jt to do the p-inverse.
+ Jt = JK;
+ svd.compute(Jt);
+ // TODO the two next lines could be replaced by
+ Eigen::dampedInverse(svd, Jp, th);
+ if (PrevProj != NULL)
+ PJp.noalias() = (*PrevProj) * Jp;
+ else
+ PJp.noalias() = Jp;
+
+ /* --- COMPUTE ERR --- */
+ const dynamicgraph::Vector &Herr(err);
+
+ /* --- COMPUTE CONTROL --- */
+ control.noalias() += PJp * Herr;
+
+ /* --- TRACE --- */
+ sotDEBUG(45) << "Pgrad = " << (PJp * Herr) << endl;
+ if (PrevProj != NULL) {
+ sotDEBUG(45) << "P = " << *PrevProj << endl;
}
+ sotDEBUG(45) << "Jp = " << Jp << endl;
+ sotDEBUG(45) << "PJp = " << PJp << endl;
+ }
sotDEBUGOUT(15);
return control;
}
-
-
-dynamicgraph::Matrix& Sot::
-computeConstraintProjector( dynamicgraph::Matrix& ProjK, const int& time )
-{
+dynamicgraph::Matrix &
+Sot::computeConstraintProjector(dynamicgraph::Matrix &ProjK, const int &time) {
sotDEBUGIN(15);
const dynamicgraph::Matrix *Jptr;
- if( 0==constraintList.size() )
- {
- ProjK.resize( 0, nbJoints );
- sotDEBUGOUT(15);
- return ProjK;
- }
- else if( 1==constraintList.size() )
- { Jptr = &(*constraintList.begin())->jacobianSOUT(time); }
- else
- {
- SOT_THROW ExceptionTask( ExceptionTask::EMPTY_LIST,
- "Not implemented yet." );
- }
+ if (0 == constraintList.size()) {
+ ProjK.resize(0, nbJoints);
+ sotDEBUGOUT(15);
+ return ProjK;
+ } else if (1 == constraintList.size()) {
+ Jptr = &(*constraintList.begin())->jacobianSOUT(time);
+ } else {
+ SOT_THROW ExceptionTask(ExceptionTask::EMPTY_LIST, "Not implemented yet.");
+ }
const dynamicgraph::Matrix &J = *Jptr;
- sotDEBUG(12) << "J = "<< J;
+ sotDEBUG(12) << "J = " << J;
const Matrix::Index nJc = J.cols();
- dynamicgraph::Matrix Jff( J.rows(),ffJointIdLast-ffJointIdFirst );
- dynamicgraph::Matrix Jc( J.rows(),nJc-ffJointIdLast+ffJointIdFirst );
+ dynamicgraph::Matrix Jff(J.rows(), ffJointIdLast - ffJointIdFirst);
+ dynamicgraph::Matrix Jc(J.rows(), nJc - ffJointIdLast + ffJointIdFirst);
- Jc.leftCols (ffJointIdFirst) = J.leftCols (ffJointIdFirst);
- Jc.rightCols(nJc-ffJointIdLast) = J.rightCols (nJc-ffJointIdLast);
+ Jc.leftCols(ffJointIdFirst) = J.leftCols(ffJointIdFirst);
+ Jc.rightCols(nJc - ffJointIdLast) = J.rightCols(nJc - ffJointIdLast);
Jff = J.middleCols(ffJointIdFirst, ffJointIdLast);
- sotDEBUG(25) << "Jc = "<< Jc;
- sotDEBUG(25) << "Jff = "<< Jff;
+ sotDEBUG(25) << "Jc = " << Jc;
+ sotDEBUG(25) << "Jff = " << Jff;
- dynamicgraph::Matrix Jffinv( Jff.cols(),Jff.rows() );
+ dynamicgraph::Matrix Jffinv(Jff.cols(), Jff.rows());
Eigen::pseudoInverse(Jff, Jffinv);
- dynamicgraph::Matrix& Jffc = ProjK;
- Jffc.resize( Jffinv.rows(),Jc.cols() );
- Jffc = -Jffinv*Jc;
- sotDEBUG(15) << "Jffc = "<< Jffc;
+ dynamicgraph::Matrix &Jffc = ProjK;
+ Jffc.resize(Jffinv.rows(), Jc.cols());
+ Jffc = -Jffinv * Jc;
+ sotDEBUG(15) << "Jffc = " << Jffc;
sotDEBUGOUT(15);
return ProjK;
}
-
/* --------------------------------------------------------------------- */
/* --- DISPLAY --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void Sot::
-display( std::ostream& os ) const
-{
-
- os << "+-----------------"<<std::endl<<"+ SOT "
- << std::endl<< "+-----------------"<<std::endl;
- for ( std::list<TaskAbstract*>::const_iterator it=this->stack.begin();
- this->stack.end()!=it;++it )
- {
- os << "| " << (*it)->getName() <<std::endl;
- }
- os<< "+-----------------"<<std::endl;
- if( taskGradient )
- {
- os << "| {Grad} " <<taskGradient->getName() << std::endl;
- os<< "+-----------------"<<std::endl;
- }
- for( ConstraintListType::const_iterator it = constraintList.begin();
- it!=constraintList.end();++it )
- { os<< "| K: "<< (*it)->getName() << endl; }
-
-
+void Sot::display(std::ostream &os) const {
+
+ os << "+-----------------" << std::endl
+ << "+ SOT " << std::endl
+ << "+-----------------" << std::endl;
+ for (std::list<TaskAbstract *>::const_iterator it = this->stack.begin();
+ this->stack.end() != it; ++it) {
+ os << "| " << (*it)->getName() << std::endl;
+ }
+ os << "+-----------------" << std::endl;
+ if (taskGradient) {
+ os << "| {Grad} " << taskGradient->getName() << std::endl;
+ os << "+-----------------" << std::endl;
+ }
+ for (ConstraintListType::const_iterator it = constraintList.begin();
+ it != constraintList.end(); ++it) {
+ os << "| K: " << (*it)->getName() << endl;
+ }
}
-std::ostream&
-operator<< ( std::ostream& os,const Sot& sot )
-{
+std::ostream &operator<<(std::ostream &os, const Sot &sot) {
sot.display(os);
return os;
}
@@ -811,36 +800,33 @@ operator<< ( std::ostream& os,const Sot& sot )
/* --- COMMAND --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-std::ostream& Sot::
-writeGraph( std::ostream& os ) const
-{
+std::ostream &Sot::writeGraph(std::ostream &os) const {
std::list<TaskAbstract *>::const_iterator iter;
- for( iter = stack.begin(); iter!=stack.end();++iter )
- {
- const TaskAbstract & task = **iter;
- std::list<TaskAbstract *>::const_iterator nextiter =iter;
- nextiter++;
-
- if (nextiter!=stack.end())
- {
- TaskAbstract & nexttask = **nextiter;
- os << "\t\t\t\"" << task.getName() << "\" -> \"" << nexttask.getName() << "\" [color=red]" << endl;
- }
-
+ for (iter = stack.begin(); iter != stack.end(); ++iter) {
+ const TaskAbstract &task = **iter;
+ std::list<TaskAbstract *>::const_iterator nextiter = iter;
+ nextiter++;
+
+ if (nextiter != stack.end()) {
+ TaskAbstract &nexttask = **nextiter;
+ os << "\t\t\t\"" << task.getName() << "\" -> \"" << nexttask.getName()
+ << "\" [color=red]" << endl;
}
+ }
- os << "\t\tsubgraph cluster_Tasks {" <<endl;
+ os << "\t\tsubgraph cluster_Tasks {" << endl;
os << "\t\t\tsubgraph \"cluster_" << getName() << "\" {" << std::endl;
- os << "\t\t\t\tcolor=lightsteelblue1; label=\"" << getName() <<"\"; style=filled;" << std::endl;
- for( iter = stack.begin(); iter!=stack.end();++iter )
- {
- const TaskAbstract & task = **iter;
- os << "\t\t\t\t\"" << task.getName()
- <<"\" [ label = \"" << task.getName() << "\" ," << std::endl
- <<"\t\t\t\t fontcolor = black, color = black, fillcolor = magenta, style=filled, shape=box ]" << std::endl;
-
- }
+ os << "\t\t\t\tcolor=lightsteelblue1; label=\"" << getName()
+ << "\"; style=filled;" << std::endl;
+ for (iter = stack.begin(); iter != stack.end(); ++iter) {
+ const TaskAbstract &task = **iter;
+ os << "\t\t\t\t\"" << task.getName() << "\" [ label = \"" << task.getName()
+ << "\" ," << std::endl
+ << "\t\t\t\t fontcolor = black, color = black, fillcolor = magenta, "
+ "style=filled, shape=box ]"
+ << std::endl;
+ }
os << "\t\t\t}" << std::endl;
- os << "\t\t\t}" <<endl;
+ os << "\t\t\t}" << endl;
return os;
}
diff --git a/src/sot/weighted-sot.cpp b/src/sot/weighted-sot.cpp
index e92d5cbd..72018bf1 100644
--- a/src/sot/weighted-sot.cpp
+++ b/src/sot/weighted-sot.cpp
@@ -12,11 +12,11 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/weighted-sot.hh>
+#include <sot/core/debug.hh>
#include <sot/core/memory-task-sot.hh>
#include <sot/core/pool.hh>
#include <sot/core/task.hh>
-#include <sot/core/debug.hh>
+#include <sot/core/weighted-sot.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
@@ -25,58 +25,57 @@ using namespace dynamicgraph;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WeightedSot,"WSOT");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WeightedSot, "WSOT");
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTION ---------------------------------------------------- */
/* --------------------------------------------------------------------- */
-WeightedSot::
-WeightedSot( const std::string& name )
- :Sot(name)
- ,weightSIN( NULL,"sotWeightedSOT("+name+")::input(matrix)::weight" )
- ,constrainedWeightSOUT( boost::bind(&WeightedSot::computeConstrainedWeight,
- this,_1,_2),
- weightSIN<<constraintSOUT,
- "sotWeightedSOT("+name+")::input(matrix)::KweightOUT" )
- ,constrainedWeightSIN( NULL,"sotWeightedSOT("+name+")::input(matrix)::Kweight" )
- ,squareRootInvWeightSOUT( boost::bind(&WeightedSot::computeSquareRootInvWeight,this,_1,_2),
- weightSIN<<constrainedWeightSIN,
- "sotWeightedSOT("+name+")::output(matrix)::sqweight" )
- ,squareRootInvWeightSIN( &squareRootInvWeightSOUT,
- "sotWeightedSOT("+name+")::input(matrix)::sqweightIN" )
-{
+WeightedSot::WeightedSot(const std::string &name)
+ : Sot(name),
+ weightSIN(NULL, "sotWeightedSOT(" + name + ")::input(matrix)::weight"),
+ constrainedWeightSOUT(
+ boost::bind(&WeightedSot::computeConstrainedWeight, this, _1, _2),
+ weightSIN << constraintSOUT,
+ "sotWeightedSOT(" + name + ")::input(matrix)::KweightOUT"),
+ constrainedWeightSIN(NULL, "sotWeightedSOT(" + name +
+ ")::input(matrix)::Kweight"),
+ squareRootInvWeightSOUT(
+ boost::bind(&WeightedSot::computeSquareRootInvWeight, this, _1, _2),
+ weightSIN << constrainedWeightSIN,
+ "sotWeightedSOT(" + name + ")::output(matrix)::sqweight"),
+ squareRootInvWeightSIN(&squareRootInvWeightSOUT,
+ "sotWeightedSOT(" + name +
+ ")::input(matrix)::sqweightIN") {
sotDEBUGIN(25);
- signalRegistration( weightSIN<<constrainedWeightSIN<<squareRootInvWeightSOUT
- << squareRootInvWeightSIN );
+ signalRegistration(weightSIN << constrainedWeightSIN
+ << squareRootInvWeightSOUT
+ << squareRootInvWeightSIN);
- controlSOUT.setFunction( boost::bind(&WeightedSot::computeWeightedControlLaw,
- this,_1,_2) );
- controlSOUT.addDependency( squareRootInvWeightSIN );
+ controlSOUT.setFunction(
+ boost::bind(&WeightedSot::computeWeightedControlLaw, this, _1, _2));
+ controlSOUT.addDependency(squareRootInvWeightSIN);
constrainedWeightSIN.plug(&constrainedWeightSOUT);
sotDEBUGOUT(25);
}
-
/* --------------------------------------------------------------------- */
/* --- A inv ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>
-dynamicgraph::Matrix& WeightedSot::
-computeSquareRootInvWeight( dynamicgraph::Matrix& S5i,const int& time )
-{
+dynamicgraph::Matrix &
+WeightedSot::computeSquareRootInvWeight(dynamicgraph::Matrix &S5i,
+ const int &time) {
sotDEBUGIN(15);
- const dynamicgraph::Matrix& A = constrainedWeightSIN( time );
- if( A.cols()!= A.rows() )
- {
- SOT_THROW ExceptionTask( ExceptionTask::MATRIX_SIZE,
- "Weight matrix should be square.","" );
- }
+ const dynamicgraph::Matrix &A = constrainedWeightSIN(time);
+ if (A.cols() != A.rows()) {
+ SOT_THROW ExceptionTask(ExceptionTask::MATRIX_SIZE,
+ "Weight matrix should be square.", "");
+ }
sotDEBUG(25) << "KA = " << A << endl;
/* Decomposition of Choleski:
@@ -86,25 +85,24 @@ computeSquareRootInvWeight( dynamicgraph::Matrix& S5i,const int& time )
* of A minus the scalar product of column i and j of s, divided by the diag
* of S: s_ij = (a_ij - sum(k=0:i) s_ik.s_jk ) / s_ii .
*/
- dynamicgraph::Matrix S5( A.rows(),A.rows() ); S5.setZero();
- //S5.resize( A.rows(),A.rows() ); S5.setZero();
- for( unsigned int i=0;i<A.cols();++i )
- {
- double x=A(i,i);
- for( unsigned int k=0;k<i;++k )
- x-=S5(k,i)*S5(k,i);
- double sq=sqrt(x);
- S5(i,i)=sq;
-
- for( unsigned int j=i+1;j<A.cols();++j )
- {
- x=A(i,j);
- for( unsigned int k=0;k<i;++k )
- x-=S5(k,i)*S5(k,j);
- S5(i,j)=x/sq;
- }
+ dynamicgraph::Matrix S5(A.rows(), A.rows());
+ S5.setZero();
+ // S5.resize( A.rows(),A.rows() ); S5.setZero();
+ for (unsigned int i = 0; i < A.cols(); ++i) {
+ double x = A(i, i);
+ for (unsigned int k = 0; k < i; ++k)
+ x -= S5(k, i) * S5(k, i);
+ double sq = sqrt(x);
+ S5(i, i) = sq;
+
+ for (unsigned int j = i + 1; j < A.cols(); ++j) {
+ x = A(i, j);
+ for (unsigned int k = 0; k < i; ++k)
+ x -= S5(k, i) * S5(k, j);
+ S5(i, j) = x / sq;
}
- sotDEBUG(20) << "S = " <<S5 << endl;
+ }
+ sotDEBUG(20) << "S = " << S5 << endl;
/* S=Inversion of a upper-triangular matrix A.
* (1) the diag of the inverse is the inverse of the diag: s_ii = 1/a_ii.
@@ -115,75 +113,77 @@ computeSquareRootInvWeight( dynamicgraph::Matrix& S5i,const int& time )
* S*A. p_ik = [S*A]_ik = 0 = sum(k=i:j) s_ik a_kj = 0. Then
* s_ij = - [ sum(k=i:j-1) s_ik a_kj ] / a_ii
*/
- S5i.resize( A.rows(),A.rows() ); S5i.setZero();
- for( unsigned int l=0;l<A.rows();++l )
- for( unsigned int i=0;i<A.cols();++i )
- {
- unsigned int j=i+l; if(j>=A.cols()) continue;
- if( j==i ) S5i(j,j)=1/S5(j,j);
- else
- {
- S5i(i,j)=0;
- for( unsigned int k=i;k<j;k++ )
- { S5i(i,j) -= S5(k,j)*S5i(i,k); }
- S5i(i,j)/=S5(j,j);
- }
+ S5i.resize(A.rows(), A.rows());
+ S5i.setZero();
+ for (unsigned int l = 0; l < A.rows(); ++l)
+ for (unsigned int i = 0; i < A.cols(); ++i) {
+ unsigned int j = i + l;
+ if (j >= A.cols())
+ continue;
+ if (j == i)
+ S5i(j, j) = 1 / S5(j, j);
+ else {
+ S5i(i, j) = 0;
+ for (unsigned int k = i; k < j; k++) {
+ S5i(i, j) -= S5(k, j) * S5i(i, k);
+ }
+ S5i(i, j) /= S5(j, j);
}
- sotDEBUG(15) << "Si = " <<S5i << endl;
+ }
+ sotDEBUG(15) << "Si = " << S5i << endl;
sotDEBUGOUT(15);
return S5i;
}
-
/* --------------------------------------------------------------------- */
/* --- CONTROL --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-dynamicgraph::Matrix& WeightedSot::
-computeConstrainedWeight( dynamicgraph::Matrix& KAK,const int& iterTime )
-{
+dynamicgraph::Matrix &
+WeightedSot::computeConstrainedWeight(dynamicgraph::Matrix &KAK,
+ const int &iterTime) {
sotDEBUGIN(15);
const dynamicgraph::Matrix &K = constraintSOUT(iterTime);
const dynamicgraph::Matrix &A = weightSIN(iterTime);
const unsigned int mJ = K.cols();
-// const unsigned int rhand=28, lhand=40;
-// for( unsigned int i=0;i<6;++i )
-// for( unsigned int j=0;j<mJ+6;++j )
-// {
-// if( i+rhand==j ) A(j,j)=1; else
-// A(i+rhand,j) = A(j,rhand+i) = 0.;
-// if( i+lhand==j ) A(j,j)=1; else
-// A(i+lhand,j) = A(j,lhand+i) = 0.;
-// }
-
- sotDEBUG(35) << "Kff = " << K << endl;
- sotDEBUG(35) << "A = " << A << endl;
-
- KAK.resize( mJ,mJ ); KAK.fill(0);
+ // const unsigned int rhand=28, lhand=40;
+ // for( unsigned int i=0;i<6;++i )
+ // for( unsigned int j=0;j<mJ+6;++j )
+ // {
+ // if( i+rhand==j ) A(j,j)=1; else
+ // A(i+rhand,j) = A(j,rhand+i) = 0.;
+ // if( i+lhand==j ) A(j,j)=1; else
+ // A(i+lhand,j) = A(j,lhand+i) = 0.;
+ // }
+
+ sotDEBUG(35) << "Kff = " << K << endl;
+ sotDEBUG(35) << "A = " << A << endl;
+
+ KAK.resize(mJ, mJ);
+ KAK.fill(0);
{
- dynamicgraph::Matrix KA( mJ,6 ); KA.fill( 0. );
- for( unsigned int i=0;i<mJ;++i )
- for( unsigned int j=0;j<6;++j )
- for( unsigned int k=0;k<6;++k )
- KA(i,j) += K(k,i)*A(k,j);
-
- for( unsigned int i=0;i<mJ;++i )
- for( unsigned int j=0;j<mJ;++j )
- {
- for( unsigned int k=0;k<6;++k )
- {
- KAK(i,j) += KA(i,k)*K(k,j);
- const double x = A(i+6,k)*K(k,j);
- KAK(i,j) += x;
- KAK(j,i) += x;
- }
- KAK(i,j) += A(i+6,j+6);
- }
+ dynamicgraph::Matrix KA(mJ, 6);
+ KA.fill(0.);
+ for (unsigned int i = 0; i < mJ; ++i)
+ for (unsigned int j = 0; j < 6; ++j)
+ for (unsigned int k = 0; k < 6; ++k)
+ KA(i, j) += K(k, i) * A(k, j);
+
+ for (unsigned int i = 0; i < mJ; ++i)
+ for (unsigned int j = 0; j < mJ; ++j) {
+ for (unsigned int k = 0; k < 6; ++k) {
+ KAK(i, j) += KA(i, k) * K(k, j);
+ const double x = A(i + 6, k) * K(k, j);
+ KAK(i, j) += x;
+ KAK(j, i) += x;
+ }
+ KAK(i, j) += A(i + 6, j + 6);
+ }
}
- sotDEBUG(35) << "KAK = " << KAK << endl;
+ sotDEBUG(35) << "KAK = " << KAK << endl;
return KAK;
}
/* --------------------------------------------------------------------- */
@@ -196,46 +196,62 @@ computeConstrainedWeight( dynamicgraph::Matrix& KAK,const int& iterTime )
//#define WITH_CHRONO
-#ifdef WITH_CHRONO
-# define sotINIT_CHRONO1 struct timeval t0,t1; double dt
-# define sotSTART_CHRONO1 gettimeofday(&t0,NULL)
-# define sotCHRONO1 \
- gettimeofday(&t1,NULL);\
- dt = ( (t1.tv_sec-t0.tv_sec) * 1000.\
- + (t1.tv_usec-t0.tv_usec+0.) / 1000. );\
- sotDEBUG(1) << "dt: "<< dt
-# define sotINITPARTCOUNTERS struct timeval tpart0
-# define sotSTARTPARTCOUNTERS gettimeofday(&tpart0,NULL)
-# define sotCOUNTER(nbc1,nbc2) \
- gettimeofday(&tpart##nbc2,NULL); \
- dt##nbc2 += ( (tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000. \
- + (tpart##nbc2.tv_usec-tpart##nbc1.tv_usec+0.) / 1000. )
-# define sotINITCOUNTER(nbc1) \
- struct timeval tpart##nbc1; double dt##nbc1=0;
+#ifdef WITH_CHRONO
+#define sotINIT_CHRONO1 \
+ struct timeval t0, t1; \
+ double dt
+#define sotSTART_CHRONO1 gettimeofday(&t0, NULL)
+#define sotCHRONO1 \
+ gettimeofday(&t1, NULL); \
+ dt = ((t1.tv_sec - t0.tv_sec) * 1000. + \
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.); \
+ sotDEBUG(1) << "dt: " << dt
+#define sotINITPARTCOUNTERS struct timeval tpart0
+#define sotSTARTPARTCOUNTERS gettimeofday(&tpart0, NULL)
+#define sotCOUNTER(nbc1, nbc2) \
+ gettimeofday(&tpart##nbc2, NULL); \
+ dt##nbc2 += ((tpart##nbc2.tv_sec - tpart##nbc1.tv_sec) * 1000. + \
+ (tpart##nbc2.tv_usec - tpart##nbc1.tv_usec + 0.) / 1000.)
+#define sotINITCOUNTER(nbc1) \
+ struct timeval tpart##nbc1; \
+ double dt##nbc1 = 0;
#else // #ifdef WITH_CHRONO
-# define sotINIT_CHRONO1
-# define sotSTART_CHRONO1
-# define sotCHRONO1 if(1) ; else std::cout
-# define sotINITPARTCOUNTERS
-# define sotSTARTPARTCOUNTERS
-# define sotCOUNTER(nbc1,nbc2)
-# define sotINITCOUNTER(nbc1)
+#define sotINIT_CHRONO1
+#define sotSTART_CHRONO1
+#define sotCHRONO1 \
+ if (1) \
+ ; \
+ else \
+ std::cout
+#define sotINITPARTCOUNTERS
+#define sotSTARTPARTCOUNTERS
+#define sotCOUNTER(nbc1, nbc2)
+#define sotINITCOUNTER(nbc1)
#endif // #ifdef WITH_CHRONO
-
-
-dynamicgraph::Vector& WeightedSot::
-computeWeightedControlLaw( dynamicgraph::Vector& control,const int& iterTime )
-{
+dynamicgraph::Vector &
+WeightedSot::computeWeightedControlLaw(dynamicgraph::Vector &control,
+ const int &iterTime) {
sotDEBUGIN(15);
- sotDEBUGF(5, " --- Time %d -------------------", iterTime );
+ sotDEBUGF(5, " --- Time %d -------------------", iterTime);
- sotINIT_CHRONO1; sotINITPARTCOUNTERS;
+ sotINIT_CHRONO1;
+ sotINITPARTCOUNTERS;
sotINITCOUNTER(0b);
- sotINITCOUNTER(1); sotINITCOUNTER(2);sotINITCOUNTER(3);sotINITCOUNTER(4);
- sotINITCOUNTER(5); sotINITCOUNTER(6);sotINITCOUNTER(7);
- sotINITCOUNTER(8); sotINITCOUNTER(9);
- sotINITCOUNTER(4a); sotINITCOUNTER(4b); sotINITCOUNTER(4c); sotINITCOUNTER(4d);sotINITCOUNTER(4e);
+ sotINITCOUNTER(1);
+ sotINITCOUNTER(2);
+ sotINITCOUNTER(3);
+ sotINITCOUNTER(4);
+ sotINITCOUNTER(5);
+ sotINITCOUNTER(6);
+ sotINITCOUNTER(7);
+ sotINITCOUNTER(8);
+ sotINITCOUNTER(9);
+ sotINITCOUNTER(4a);
+ sotINITCOUNTER(4b);
+ sotINITCOUNTER(4c);
+ sotINITCOUNTER(4d);
+ sotINITCOUNTER(4e);
sotSTART_CHRONO1;
sotSTARTPARTCOUNTERS;
@@ -244,247 +260,255 @@ computeWeightedControlLaw( dynamicgraph::Vector& control,const int& iterTime )
const double &th = inversionThresholdSIN(iterTime);
const dynamicgraph::Matrix &K = constraintSOUT(iterTime);
const unsigned int mJ = K.cols();
- //dynamicgraph::Matrix A = weightSIN(iterTime);
+ // dynamicgraph::Matrix A = weightSIN(iterTime);
const dynamicgraph::Matrix &S5i = squareRootInvWeightSIN(iterTime);
- dynamicgraph::Matrix Ai(mJ,mJ);
- Ai = S5i.multiply*(S5i.transpose()); // TODO: Optimize by considering the triangular shape!
+ dynamicgraph::Matrix Ai(mJ, mJ);
+ Ai = S5i.multiply *
+ (S5i.transpose()); // TODO: Optimize by considering the triangular shape!
sotDEBUG(35) << "Ai = " << Ai << endl;
/* --- Q0 --- */
/* --- Q0 --- */
try {
- control = q0SIN( iterTime );
+ control = q0SIN(iterTime);
sotDEBUG(15) << "initial velocity q0 = " << control << endl;
- if( mJ!=control.size() ) { control.resize( mJ ); control.fill(.0); }
- }
- catch (...)
- {
- if( mJ!=control.size() ) { control.resize( mJ ); }
- control.setZero();
- sotDEBUG(25) << "No initial velocity." <<endl;
+ if (mJ != control.size()) {
+ control.resize(mJ);
+ control.fill(.0);
}
-
+ } catch (...) {
+ if (mJ != control.size()) {
+ control.resize(mJ);
+ }
+ control.setZero();
+ sotDEBUG(25) << "No initial velocity." << endl;
+ }
/* --- Main Loop --- */
/* --- Main Loop --- */
unsigned int iterTask = 0;
- for( StackType::iterator iter = stack.begin(); iter!=stack.end();++iter )
- {
- sotCOUNTER(0,0b); // Direct Dynamic
- sotDEBUGF(5,"Rank %d.",iterTask);
- TaskAbstract & task = **iter;
- const dynamicgraph::Matrix &JacRO = task.jacobianSOUT(iterTime);
- const dynamicgraph::Vector err;
- Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
- const unsigned int nJ = JacRO.rows();
- sotCOUNTER(0b,1); // Direct Dynamic
-
- /* Init memory. */
- MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
- if( NULL==mem )
- {
- if( NULL!=task.memoryInternal ) delete task.memoryInternal;
- mem = new MemoryTaskSOT( task.getName()+"_memSOT",nJ,mJ );
- task.memoryInternal = mem;
- }
-
- dynamicgraph::Matrix Jp,V; unsigned int rankJ;
- dynamicgraph::Matrix Jac(nJ,mJ);
- dynamicgraph::Matrix Jt(nJ,mJ);
+ for (StackType::iterator iter = stack.begin(); iter != stack.end(); ++iter) {
+ sotCOUNTER(0, 0b); // Direct Dynamic
+ sotDEBUGF(5, "Rank %d.", iterTask);
+ TaskAbstract &task = **iter;
+ const dynamicgraph::Matrix &JacRO = task.jacobianSOUT(iterTime);
+ const dynamicgraph::Vector err;
+ Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
+ const unsigned int nJ = JacRO.rows();
+ sotCOUNTER(0b, 1); // Direct Dynamic
+
+ /* Init memory. */
+ MemoryTaskSOT *mem = dynamic_cast<MemoryTaskSOT *>(task.memoryInternal);
+ if (NULL == mem) {
+ if (NULL != task.memoryInternal)
+ delete task.memoryInternal;
+ mem = new MemoryTaskSOT(task.getName() + "_memSOT", nJ, mJ);
+ task.memoryInternal = mem;
+ }
- if( (task.jacobianSOUT.getTime()>mem->jacobianInvSINOUT.getTime())
- ||(mem->jacobianInvSINOUT.accessCopy().rows()!=mJ)
- ||(mem->jacobianInvSINOUT.accessCopy().cols()!=nJ)
- ||(task.jacobianSOUT.getTime()>mem->jacobianConstrainedSINOUT.getTime())
- ||(task.jacobianSOUT.getTime()>mem->rankSINOUT.getTime())
- ||(task.jacobianSOUT.getTime()>mem->singularBaseImageSINOUT.getTime()) )
- {
- sotDEBUG(15) <<"Recompute inverse."<<endl;
-
- /* --- FIRST ALLOCS --- */
- const unsigned int FF_SIZE=JacRO.cols()-mJ;
- dynamicgraph::Matrix Jff( nJ,FF_SIZE );
- dynamicgraph::Matrix Jact( nJ,mJ );
- Jp.resize( mJ,nJ );
- /***/sotCOUNTER(1,2); // first allocs
-
- /* --- COMPUTE JK --- */
- for( unsigned int i=0;i<nJ;++i )
- {
- for( unsigned int j=0;j<FF_SIZE;++j ) Jff(i,j)=JacRO(i,j);
- for( unsigned int j=FF_SIZE;j<JacRO.cols();++j )
- Jact(i,j-FF_SIZE)=JacRO(i,j);
- }
- Jac = Jff*K;
- Jac+=Jact;
- /***/sotCOUNTER(2,3); // compute JK
-
- /* --- COMPUTE Jt --- */
- if( mJ==P.cols() ) Jt = Jac*P;
- else { Jt = Jac; }
- /***/sotCOUNTER(3,4); // compute Jt
-
- /* --- COMPUTE S --- */
- dynamicgraph::Matrix Kact(mJ,mJ); Kact=S5i;
- Task* taskSpec = dynamic_cast<Task*>( &task );
- if( NULL!=taskSpec )
- {
- const Flags& controlSelec = taskSpec->controlSelectionSIN( iterTime );
- sotDEBUG(25) << "Control selection = " << controlSelec <<endl;
- if( controlSelec )
- {
- if(!controlSelec)
- {
- /***/sotCOUNTER(4,4a); // compute Kh
- sotDEBUG(15) << "Control selection."<<endl;
- std::vector<unsigned int> unactiveList;
- sotDEBUG(25) << "selec"<<iterTask<<" = [ ";
- for( unsigned int i=0;i<mJ;++i )
- {
- if(! controlSelec(i) )
- {
- if(sotDEBUG_ENABLE(25)) sotDEBUGFLOW.outputbuffer << i <<" ";
- unactiveList.push_back(i);
- }
- }
- if(sotDEBUG_ENABLE(25)) sotDEBUGFLOW.outputbuffer << "]" << endl ;
-
- const unsigned int unactiveSize = unactiveList.size();
- /***/sotCOUNTER(4a,4b); // compute Kh
-
- /* Q = H*Ai*H, H being the unactivation matrix. */
- dynamicgraph::Matrix Q(unactiveSize,unactiveSize);
- dynamicgraph::Matrix Sir(unactiveSize,mJ);
- for( unsigned int i=0;i<unactiveSize;++i )
- {
- for( unsigned int j=0;j<unactiveSize;++j )
- Q(i,j)=Ai(unactiveList[i],unactiveList[j]);
-
- for( unsigned int j=0;j<mJ;++j )
- Sir(i,j) = S5i(unactiveList[i],j);
- }
- sotDEBUG(25) << "Q"<<iterTask<<" = " << Q << endl;
- sotDEBUG(25) << "Sr"<<iterTask<<" = " << Sir << endl;
- /***/sotCOUNTER(4b,4c); // compute Kh
-
- /* Qi = inv(Q), always positive. */
- dynamicgraph::Matrix Qi(unactiveSize,unactiveSize);
- Q.inverse(Qi);
- sotDEBUG(25) << "Qi"<<iterTask<<" = " << Qi << endl;
- /***/sotCOUNTER(4c,4d); // compute Kh
-
- /* Kact = (HSi)^+ HSi = Si_red^T Qi Si_red. */
- dynamicgraph::Matrix SrQi(mJ,unactiveSize);
- dynamicgraph::Matrix SrQiSr(mJ,mJ);
- dynamicgraph::Matrix ArQiSr(mJ,mJ);
- SrQi = (Sir.transpose())*Qi;
- SrQiSr = SrQi*Sir;
- ArQiSr = S5i*SrQiSr;
- Kact-=ArQiSr; // TODO: optimizer ce merdier! Kact est sparse!
-
- sotDEBUG(15) << "Kact"<<iterTask<<" = "<<Kact<<endl;
- /***/sotCOUNTER(4d,4e); // compute Kh
- }
- else
- {
- sotDEBUG(15) << "S is equal to Id."<<endl;
- }
- }
- else
- {
- sotDEBUG(15) << "Task not activated."<<endl;
- Jt *= 0;
- }
- }
- /***/sotCOUNTER(4,5); // compute Kh
-
- /* --- PINV --- */
- dynamicgraph::Matrix U; dynamicgraph::Vector S;
- dynamicgraph::Matrix JtA( Jt.rows(),Jt.cols() );
- dynamicgraph::Matrix JtAp( Jt.cols(),Jt.rows() );
-
- JtA = Jt * Kact;
- JtA.pseudoInverse( JtAp,th,&U,&S,&V );
- Jp = S5i * JtAp; // TODO: S5i*JtAp = Kact*JtAp, and Kact is sparse.
-
- if( mJ==P.cols() ) Jp = P*Jp; // P is not idempotent
- /***/sotCOUNTER(5,6); // PINV
-
- /* --- RANK --- */
- {
- const unsigned int Jmax = S.size(); rankJ=0;
- for( unsigned i=0;i<Jmax;++i ) { if( S(i)>th ) rankJ++; }
- }
-
- sotDEBUG(45) << "control"<<iterTask<<" = "<<control<<endl;
- sotDEBUG(25) << "J"<<iterTask<<" = "<<JacRO<<endl;
- sotDEBUG(25) << "JK"<<iterTask<<" = "<<Jac<<endl;
- sotDEBUG(25) << "Jt"<<iterTask<<" = "<<Jt<<endl;
- sotDEBUG(25) << "JtA"<<iterTask<<" = "<<JtA<<endl;
- sotDEBUG(15) << "Jp"<<iterTask<<" = "<<Jp<<endl;
- sotDEBUG(15) << "e"<<iterTask<<" = "<<err<<endl;
- sotDEBUG(45) << "JJp"<<iterTask<<" = "<< Jac*Jp <<endl;
- sotDEBUG(45) << "U"<<iterTask<<" = "<< U<<endl;
- sotDEBUG(45) << "Svd"<<iterTask<<" = "<< S<<endl;
- sotDEBUG(45) << "V"<<iterTask<<" = "<< V<<endl;
-
- mem->jacobianInvSINOUT = Jp;
- mem->jacobianInvSINOUT.setTime( iterTime );
- mem->jacobianConstrainedSINOUT = Jac;
- mem->jacobianConstrainedSINOUT.setTime( iterTime );
- mem->singularBaseImageSINOUT = V;
- mem->singularBaseImageSINOUT.setTime( iterTime );
- mem->rankSINOUT = rankJ;
- mem->rankSINOUT.setTime( iterTime );
-
- sotDEBUG(25)<<"Inverse recomputed."<<endl;
-
- } else {
- sotDEBUG(15)<<"Inverse not recomputed."<<endl;
- rankJ = mem->rankSINOUT.accessCopy();
- Jac = mem->jacobianConstrainedSINOUT.accessCopy();
- Jp = mem->jacobianInvSINOUT.accessCopy();
- V = mem->singularBaseImageSINOUT.accessCopy();
+ dynamicgraph::Matrix Jp, V;
+ unsigned int rankJ;
+ dynamicgraph::Matrix Jac(nJ, mJ);
+ dynamicgraph::Matrix Jt(nJ, mJ);
+
+ if ((task.jacobianSOUT.getTime() > mem->jacobianInvSINOUT.getTime()) ||
+ (mem->jacobianInvSINOUT.accessCopy().rows() != mJ) ||
+ (mem->jacobianInvSINOUT.accessCopy().cols() != nJ) ||
+ (task.jacobianSOUT.getTime() >
+ mem->jacobianConstrainedSINOUT.getTime()) ||
+ (task.jacobianSOUT.getTime() > mem->rankSINOUT.getTime()) ||
+ (task.jacobianSOUT.getTime() >
+ mem->singularBaseImageSINOUT.getTime())) {
+ sotDEBUG(15) << "Recompute inverse." << endl;
+
+ /* --- FIRST ALLOCS --- */
+ const unsigned int FF_SIZE = JacRO.cols() - mJ;
+ dynamicgraph::Matrix Jff(nJ, FF_SIZE);
+ dynamicgraph::Matrix Jact(nJ, mJ);
+ Jp.resize(mJ, nJ);
+ /***/ sotCOUNTER(1, 2); // first allocs
+
+ /* --- COMPUTE JK --- */
+ for (unsigned int i = 0; i < nJ; ++i) {
+ for (unsigned int j = 0; j < FF_SIZE; ++j)
+ Jff(i, j) = JacRO(i, j);
+ for (unsigned int j = FF_SIZE; j < JacRO.cols(); ++j)
+ Jact(i, j - FF_SIZE) = JacRO(i, j);
+ }
+ Jac = Jff * K;
+ Jac += Jact;
+ /***/ sotCOUNTER(2, 3); // compute JK
+
+ /* --- COMPUTE Jt --- */
+ if (mJ == P.cols())
+ Jt = Jac * P;
+ else {
+ Jt = Jac;
+ }
+ /***/ sotCOUNTER(3, 4); // compute Jt
+
+ /* --- COMPUTE S --- */
+ dynamicgraph::Matrix Kact(mJ, mJ);
+ Kact = S5i;
+ Task *taskSpec = dynamic_cast<Task *>(&task);
+ if (NULL != taskSpec) {
+ const Flags &controlSelec = taskSpec->controlSelectionSIN(iterTime);
+ sotDEBUG(25) << "Control selection = " << controlSelec << endl;
+ if (controlSelec) {
+ if (!controlSelec) {
+ /***/ sotCOUNTER(4, 4a); // compute Kh
+ sotDEBUG(15) << "Control selection." << endl;
+ std::vector<unsigned int> unactiveList;
+ sotDEBUG(25) << "selec" << iterTask << " = [ ";
+ for (unsigned int i = 0; i < mJ; ++i) {
+ if (!controlSelec(i)) {
+ if (sotDEBUG_ENABLE(25))
+ sotDEBUGFLOW.outputbuffer << i << " ";
+ unactiveList.push_back(i);
+ }
+ }
+ if (sotDEBUG_ENABLE(25))
+ sotDEBUGFLOW.outputbuffer << "]" << endl;
+
+ const unsigned int unactiveSize = unactiveList.size();
+ /***/ sotCOUNTER(4a, 4b); // compute Kh
+
+ /* Q = H*Ai*H, H being the unactivation matrix. */
+ dynamicgraph::Matrix Q(unactiveSize, unactiveSize);
+ dynamicgraph::Matrix Sir(unactiveSize, mJ);
+ for (unsigned int i = 0; i < unactiveSize; ++i) {
+ for (unsigned int j = 0; j < unactiveSize; ++j)
+ Q(i, j) = Ai(unactiveList[i], unactiveList[j]);
+
+ for (unsigned int j = 0; j < mJ; ++j)
+ Sir(i, j) = S5i(unactiveList[i], j);
+ }
+ sotDEBUG(25) << "Q" << iterTask << " = " << Q << endl;
+ sotDEBUG(25) << "Sr" << iterTask << " = " << Sir << endl;
+ /***/ sotCOUNTER(4b, 4c); // compute Kh
+
+ /* Qi = inv(Q), always positive. */
+ dynamicgraph::Matrix Qi(unactiveSize, unactiveSize);
+ Q.inverse(Qi);
+ sotDEBUG(25) << "Qi" << iterTask << " = " << Qi << endl;
+ /***/ sotCOUNTER(4c, 4d); // compute Kh
+
+ /* Kact = (HSi)^+ HSi = Si_red^T Qi Si_red. */
+ dynamicgraph::Matrix SrQi(mJ, unactiveSize);
+ dynamicgraph::Matrix SrQiSr(mJ, mJ);
+ dynamicgraph::Matrix ArQiSr(mJ, mJ);
+ SrQi = (Sir.transpose()) * Qi;
+ SrQiSr = SrQi * Sir;
+ ArQiSr = S5i * SrQiSr;
+ Kact -= ArQiSr; // TODO: optimizer ce merdier! Kact est sparse!
+
+ sotDEBUG(15) << "Kact" << iterTask << " = " << Kact << endl;
+ /***/ sotCOUNTER(4d, 4e); // compute Kh
+ } else {
+ sotDEBUG(15) << "S is equal to Id." << endl;
+ }
+ } else {
+ sotDEBUG(15) << "Task not activated." << endl;
+ Jt *= 0;
+ }
}
+ /***/ sotCOUNTER(4, 5); // compute Kh
- /***/sotCOUNTER(6,7); // TRACE
+ /* --- PINV --- */
+ dynamicgraph::Matrix U;
+ dynamicgraph::Vector S;
+ dynamicgraph::Matrix JtA(Jt.rows(), Jt.cols());
+ dynamicgraph::Matrix JtAp(Jt.cols(), Jt.rows());
- /* --- COMPUTE QDOT AND P --- */
- /*DEBUG*/// if( iterTask==0 ) control += Jp*err; else
- //Jp*err;
- sotDEBUG(15) << "G = " << Jp<<endl;
- sotDEBUG(15) << "e = " << err<<endl;
- sotDEBUG(15) << "Ge = " << Jp*err<<endl;
- sotDEBUG(15) << "d = " << Jac*control<<endl;
- control += Jp*(err - Jac*control);
- /***/sotCOUNTER(7,8); // QDOT
+ JtA = Jt * Kact;
+ JtA.pseudoInverse(JtAp, th, &U, &S, &V);
+ Jp = S5i * JtAp; // TODO: S5i*JtAp = Kact*JtAp, and Kact is sparse.
+ if (mJ == P.cols())
+ Jp = P * Jp; // P is not idempotent
+ /***/ sotCOUNTER(5, 6); // PINV
- if( mJ!=P.cols() ) { P.resize( mJ,mJ ); P.setIdentity(); }
+ /* --- RANK --- */
{
- dynamicgraph::Matrix Kp( mJ,mJ );
- Kp = Jp*Jt;
- P -= Kp;
+ const unsigned int Jmax = S.size();
+ rankJ = 0;
+ for (unsigned i = 0; i < Jmax; ++i) {
+ if (S(i) > th)
+ rankJ++;
+ }
}
- sotCOUNTER(8,9); // Projo
- sotDEBUG(15) << "q"<<iterTask<<" = "<<control<<std::endl;
- sotDEBUG(25) << "P"<<iterTask<<" = "<< P <<endl;
+ sotDEBUG(45) << "control" << iterTask << " = " << control << endl;
+ sotDEBUG(25) << "J" << iterTask << " = " << JacRO << endl;
+ sotDEBUG(25) << "JK" << iterTask << " = " << Jac << endl;
+ sotDEBUG(25) << "Jt" << iterTask << " = " << Jt << endl;
+ sotDEBUG(25) << "JtA" << iterTask << " = " << JtA << endl;
+ sotDEBUG(15) << "Jp" << iterTask << " = " << Jp << endl;
+ sotDEBUG(15) << "e" << iterTask << " = " << err << endl;
+ sotDEBUG(45) << "JJp" << iterTask << " = " << Jac * Jp << endl;
+ sotDEBUG(45) << "U" << iterTask << " = " << U << endl;
+ sotDEBUG(45) << "Svd" << iterTask << " = " << S << endl;
+ sotDEBUG(45) << "V" << iterTask << " = " << V << endl;
+
+ mem->jacobianInvSINOUT = Jp;
+ mem->jacobianInvSINOUT.setTime(iterTime);
+ mem->jacobianConstrainedSINOUT = Jac;
+ mem->jacobianConstrainedSINOUT.setTime(iterTime);
+ mem->singularBaseImageSINOUT = V;
+ mem->singularBaseImageSINOUT.setTime(iterTime);
+ mem->rankSINOUT = rankJ;
+ mem->rankSINOUT.setTime(iterTime);
+
+ sotDEBUG(25) << "Inverse recomputed." << endl;
+
+ } else {
+ sotDEBUG(15) << "Inverse not recomputed." << endl;
+ rankJ = mem->rankSINOUT.accessCopy();
+ Jac = mem->jacobianConstrainedSINOUT.accessCopy();
+ Jp = mem->jacobianInvSINOUT.accessCopy();
+ V = mem->singularBaseImageSINOUT.accessCopy();
+ }
+
+ /***/ sotCOUNTER(6, 7); // TRACE
+
+ /* --- COMPUTE QDOT AND P --- */
+ /*DEBUG*/ // if( iterTask==0 ) control += Jp*err; else
+ // Jp*err;
+ sotDEBUG(15) << "G = " << Jp << endl;
+ sotDEBUG(15) << "e = " << err << endl;
+ sotDEBUG(15) << "Ge = " << Jp * err << endl;
+ sotDEBUG(15) << "d = " << Jac * control << endl;
+ control += Jp * (err - Jac * control);
+ /***/ sotCOUNTER(7, 8); // QDOT
+
+ if (mJ != P.cols()) {
+ P.resize(mJ, mJ);
+ P.setIdentity();
+ }
+ {
+ dynamicgraph::Matrix Kp(mJ, mJ);
+ Kp = Jp * Jt;
+ P -= Kp;
+ }
+ sotCOUNTER(8, 9); // Projo
+
+ sotDEBUG(15) << "q" << iterTask << " = " << control << std::endl;
+ sotDEBUG(25) << "P" << iterTask << " = " << P << endl;
#ifdef WITH_CH
- sotDEBUG(1) << "Chrono"<<iterTask<<"_"<<iterTime<<" = [ " << endl
- <<" "<<dt0b<<" "<<dt1 <<" "<< dt2 <<" "<< dt3 <<" "
- << dt4 <<" "<<dt5 <<", "<< dt6 <<" "<< dt7
- <<" "<< dt8 <<" "<< dt9 << " ] " << endl;
- sotDEBUG(1) << "ChronoKh"<<iterTask<<"_"<<iterTime<<" = [ " << endl
- <<" "<< dt4a<<" "<< dt4b<<" "<< dt4c<<" "
- << dt4d<<" "<< dt4e<<" " << " ] " << endl;
+ sotDEBUG(1) << "Chrono" << iterTask << "_" << iterTime << " = [ " << endl
+ << " " << dt0b << " " << dt1 << " " << dt2 << " " << dt3 << " "
+ << dt4 << " " << dt5 << ", " << dt6 << " " << dt7 << " " << dt8
+ << " " << dt9 << " ] " << endl;
+ sotDEBUG(1) << "ChronoKh" << iterTask << "_" << iterTime << " = [ " << endl
+ << " " << dt4a << " " << dt4b << " " << dt4c << " " << dt4d
+ << " " << dt4e << " "
+ << " ] " << endl;
#endif // #ifdef WITH_CHRONO
- iterTask++;
- }
+ iterTask++;
+ }
sotCHRONO1 << endl;
-
sotDEBUGOUT(15);
return control;
}
diff --git a/src/task/constraint-command.h b/src/task/constraint-command.h
index 1b626646..098feef1 100644
--- a/src/task/constraint-command.h
+++ b/src/task/constraint-command.h
@@ -7,56 +7,52 @@
*/
#ifndef CONSTRAINT_COMMAND_H
- #define CONSTRAINT_COMMAND_H
+#define CONSTRAINT_COMMAND_H
- #include <boost/assign/list_of.hpp>
+#include <boost/assign/list_of.hpp>
- #include <dynamic-graph/command.h>
- #include <dynamic-graph/command-setter.h>
- #include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command.h>
namespace dynamicgraph {
- namespace sot {
- namespace command {
- namespace constraint {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
+namespace sot {
+namespace command {
+namespace constraint {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
- // Command AddJacobian
- class AddJacobian : public Command
- {
- public:
- virtual ~AddJacobian() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- AddJacobian(Constraint& entity, const std::string& docstring) :
- Command(entity, boost::assign::list_of(Value::STRING), docstring)
- {
- }
- virtual Value doExecute()
- {
- Constraint& constraint = static_cast<Constraint&>(owner());
- std::vector<Value> values = getParameterValues();
- std::string signalName = values[0].value();
- std::istringstream iss(signalName);
- SignalBase<int>& signal =
- dynamicgraph::PoolStorage::getInstance()->getSignal(iss);
- try {
- Signal< dynamicgraph::Matrix,int >& matrixSignal
- = dynamic_cast< Signal<dynamicgraph::Matrix,int>& >( signal );
- constraint.addJacobian(matrixSignal);
- } catch (const std::bad_cast& exc) {
- std::string msg(signalName + " is not of type matrix.");
- SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST, msg);
- }
- // return void
- return Value();
- }
- }; // class AddJacobian
- } // namespace constraint
- } // namespace command
- } // namespace sot
+// Command AddJacobian
+class AddJacobian : public Command {
+public:
+ virtual ~AddJacobian() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ AddJacobian(Constraint &entity, const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+ virtual Value doExecute() {
+ Constraint &constraint = static_cast<Constraint &>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string signalName = values[0].value();
+ std::istringstream iss(signalName);
+ SignalBase<int> &signal =
+ dynamicgraph::PoolStorage::getInstance()->getSignal(iss);
+ try {
+ Signal<dynamicgraph::Matrix, int> &matrixSignal =
+ dynamic_cast<Signal<dynamicgraph::Matrix, int> &>(signal);
+ constraint.addJacobian(matrixSignal);
+ } catch (const std::bad_cast &exc) {
+ std::string msg(signalName + " is not of type matrix.");
+ SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, msg);
+ }
+ // return void
+ return Value();
+ }
+}; // class AddJacobian
+} // namespace constraint
+} // namespace command
+} // namespace sot
} // namespace dynamicgraph
-#endif //CONSTRAINT_COMMAND_H
+#endif // CONSTRAINT_COMMAND_H
diff --git a/src/task/constraint.cpp b/src/task/constraint.cpp
index d36ec565..bc5b0f61 100644
--- a/src/task/constraint.cpp
+++ b/src/task/constraint.cpp
@@ -12,9 +12,9 @@
/* --------------------------------------------------------------------- */
/* SOT */
+#include <dynamic-graph/pool.h>
#include <sot/core/constraint.hh>
#include <sot/core/debug.hh>
-#include <dynamic-graph/pool.h>
using namespace std;
using namespace dynamicgraph::sot;
@@ -22,20 +22,17 @@ using namespace dynamicgraph::sot;
using namespace dynamicgraph;
#include "../src/task/constraint-command.h"
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Constraint,"Constraint");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Constraint, "Constraint");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+Constraint::Constraint(const std::string &n) : TaskAbstract(n) {
+ jacobianSOUT.setFunction(
+ boost::bind(&Constraint::computeJacobian, this, _1, _2));
-Constraint::
-Constraint( const std::string& n )
- :TaskAbstract(n)
-{
- jacobianSOUT.setFunction( boost::bind(&Constraint::computeJacobian,this,_1,_2) );
-
- signalDeregistration( taskSOUT.shortName() );
+ signalDeregistration(taskSOUT.shortName());
//
// Commands
@@ -43,36 +40,29 @@ Constraint( const std::string& n )
std::string docstring;
// Addjacobian
docstring = " \n"
- " \n"
- " Add a Jacobian to the constraint\n"
- " \n"
- " Input:\n"
- " - name of the signal conveying the Jacobian.\n"
- " \n";
+ " \n"
+ " Add a Jacobian to the constraint\n"
+ " \n"
+ " Input:\n"
+ " - name of the signal conveying the Jacobian.\n"
+ " \n";
addCommand("addJacobian",
- new command::constraint::AddJacobian(*this, docstring));
+ new command::constraint::AddJacobian(*this, docstring));
}
-
-
-void Constraint::
-addJacobian( Signal< dynamicgraph::Matrix,int >& sig )
-{
+void Constraint::addJacobian(Signal<dynamicgraph::Matrix, int> &sig) {
sotDEBUGIN(15);
jacobianList.push_back(&sig);
- jacobianSOUT.addDependency( sig );
+ jacobianSOUT.addDependency(sig);
sotDEBUGOUT(15);
}
-void Constraint::
-clearJacobianList( void )
-{
-
- for( JacobianList::iterator iter = jacobianList.begin();
- iter!=jacobianList.end(); ++iter )
- {
- Signal< dynamicgraph::Matrix,int >& s = **iter;
- jacobianSOUT.removeDependency( s );
- }
+void Constraint::clearJacobianList(void) {
+
+ for (JacobianList::iterator iter = jacobianList.begin();
+ iter != jacobianList.end(); ++iter) {
+ Signal<dynamicgraph::Matrix, int> &s = **iter;
+ jacobianSOUT.removeDependency(s);
+ }
jacobianList.clear();
}
@@ -81,55 +71,53 @@ clearJacobianList( void )
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
-dynamicgraph::Matrix& Constraint::
-computeJacobian( dynamicgraph::Matrix& J,int time )
-{
+dynamicgraph::Matrix &Constraint::computeJacobian(dynamicgraph::Matrix &J,
+ int time) {
sotDEBUG(15) << "# In {" << endl;
- if( jacobianList.empty())
- { J.resize(0,0); }
+ if (jacobianList.empty()) {
+ J.resize(0, 0);
+ }
// { throw( ExceptionTask(ExceptionTask::EMPTY_LIST,
// "Empty feature list") ) ; }
try {
- dynamicgraph::Matrix::Index dimJ = J .rows();
+ dynamicgraph::Matrix::Index dimJ = J.rows();
dynamicgraph::Matrix::Index nbc = J.cols();
- if( 0==dimJ ) {
+ if (0 == dimJ) {
// Compute the correct size
- for( JacobianList::iterator iter = jacobianList.begin();
- iter!=jacobianList.end(); ++iter )
- {
- Signal< dynamicgraph::Matrix,int >& jacobian = ** iter;
- const dynamicgraph::Matrix& partialJacobian = jacobian(time);
- dimJ += partialJacobian.rows();
- if (nbc == 0)
- nbc = partialJacobian.cols();
- else if (nbc != partialJacobian.cols()) {
- SOT_THROW ExceptionTask(ExceptionTask::NON_ADEQUATE_FEATURES,
- "Features from the list don't "
- "have compatible-size jacobians.");
- }
+ for (JacobianList::iterator iter = jacobianList.begin();
+ iter != jacobianList.end(); ++iter) {
+ Signal<dynamicgraph::Matrix, int> &jacobian = **iter;
+ const dynamicgraph::Matrix &partialJacobian = jacobian(time);
+ dimJ += partialJacobian.rows();
+ if (nbc == 0)
+ nbc = partialJacobian.cols();
+ else if (nbc != partialJacobian.cols()) {
+ SOT_THROW ExceptionTask(ExceptionTask::NON_ADEQUATE_FEATURES,
+ "Features from the list don't "
+ "have compatible-size jacobians.");
}
- J.resize (dimJ, nbc);
+ }
+ J.resize(dimJ, nbc);
}
dynamicgraph::Matrix::Index cursorJ = 0;
/* For each cell of the list, recopy value of s, s_star and error. */
- for( JacobianList::iterator iter = jacobianList.begin();
- iter!=jacobianList.end(); ++iter )
- {
- Signal< dynamicgraph::Matrix,int >& jacobian = ** iter;
+ for (JacobianList::iterator iter = jacobianList.begin();
+ iter != jacobianList.end(); ++iter) {
+ Signal<dynamicgraph::Matrix, int> &jacobian = **iter;
- /* Get s, and store it in the s vector. */
- const dynamicgraph::Matrix& partialJacobian = jacobian(time);
- const dynamicgraph::Matrix::Index nbr = partialJacobian.rows();
+ /* Get s, and store it in the s vector. */
+ const dynamicgraph::Matrix &partialJacobian = jacobian(time);
+ const dynamicgraph::Matrix::Index nbr = partialJacobian.rows();
- sotDEBUG(25) << "Jp =" <<endl<< partialJacobian<<endl;
+ sotDEBUG(25) << "Jp =" << endl << partialJacobian << endl;
- J.middleRows (cursorJ, nbr) = partialJacobian;
- cursorJ += nbr;
- }
+ J.middleRows(cursorJ, nbr) = partialJacobian;
+ cursorJ += nbr;
+ }
} catch SOT_RETHROW;
@@ -137,16 +125,14 @@ computeJacobian( dynamicgraph::Matrix& J,int time )
return J;
}
-
-
-
/* --- DISPLAY ------------------------------------------------------------ */
/* --- DISPLAY ------------------------------------------------------------ */
/* --- DISPLAY ------------------------------------------------------------ */
namespace dynamicgraph {
- namespace sot {
- std::ostream& operator<< ( std::ostream& os,const Constraint& t )
- { return os << t.name; }
- } // namespace sot
+namespace sot {
+std::ostream &operator<<(std::ostream &os, const Constraint &t) {
+ return os << t.name;
+}
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/task/gain-adaptive.cpp b/src/task/gain-adaptive.cpp
index 431e809c..0fab95c7 100644
--- a/src/task/gain-adaptive.cpp
+++ b/src/task/gain-adaptive.cpp
@@ -10,13 +10,12 @@
/* SOT */
#include <sot/core/gain-adaptive.hh>
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/core/debug.hh>
-#include <sot/core/factory.hh>
#include <sot/core/exception-signal.hh>
+#include <sot/core/factory.hh>
#include <sot/core/debug.hh>
@@ -25,109 +24,90 @@
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GainAdaptive,"GainAdaptive");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GainAdaptive, "GainAdaptive");
-const double GainAdaptive::
-ZERO_DEFAULT = .1;
-const double GainAdaptive::
-INFTY_DEFAULT = .1;
-const double GainAdaptive::
-TAN_DEFAULT = 1;
+const double GainAdaptive::ZERO_DEFAULT = .1;
+const double GainAdaptive::INFTY_DEFAULT = .1;
+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, \
+ "sotGainAdaptive(" + name + ")::output(double)::gain")
-#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()
-{
+void GainAdaptive::addCommands() {
using namespace ::dynamicgraph::command;
std::string docstring;
// Command SetConstant
- docstring = " \n"
- " setConstant\n"
- " Input:\n"
- " floating point value: value at 0. Other values are set to"
- "default.\n"
- " \n";
+ docstring =
+ " \n"
+ " setConstant\n"
+ " Input:\n"
+ " floating point value: value at 0. Other values are set to"
+ "default.\n"
+ " \n";
addCommand("setConstant",
- makeCommandVoid1(*this,&GainAdaptive::init,
- docstring));
+ 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";
- addCommand("set",
- makeCommandVoid3(*this,&GainAdaptive::init,
- docstring));
+ " 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";
- addCommand("setByPoint",
- makeCommandVoid4(*this,&GainAdaptive::initFromPassingPoint,
- docstring));
+ " 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));
}
-GainAdaptive::
-GainAdaptive( const std::string & name )
- :__SOT_GAIN_ADAPTATIVE_INIT
-{
- sotDEBUG(15) << "New gain <"<<name<<">"<<std::endl;
+GainAdaptive::GainAdaptive(const std::string &name)
+ : __SOT_GAIN_ADAPTATIVE_INIT {
+ sotDEBUG(15) << "New gain <" << name << ">" << std::endl;
init();
- Entity::signalRegistration( gainSOUT<<errorSIN );
+ Entity::signalRegistration(gainSOUT << errorSIN);
addCommands();
}
-
-GainAdaptive::
-GainAdaptive( const std::string & name,const double& lambda )
- :__SOT_GAIN_ADAPTATIVE_INIT
-{
+GainAdaptive::GainAdaptive(const std::string &name, const double &lambda)
+ : __SOT_GAIN_ADAPTATIVE_INIT {
init(lambda);
- Entity::signalRegistration( gainSOUT );
+ Entity::signalRegistration(gainSOUT);
addCommands();
}
-GainAdaptive::
-GainAdaptive( const std::string & name,
- const double& valueAt0,
- const double& valueAtInfty,
- const double& tanAt0 )
- :__SOT_GAIN_ADAPTATIVE_INIT
-{
- init(valueAt0,valueAtInfty,tanAt0);
- Entity::signalRegistration( gainSOUT );
+GainAdaptive::GainAdaptive(const std::string &name, const double &valueAt0,
+ const double &valueAtInfty, const double &tanAt0)
+ : __SOT_GAIN_ADAPTATIVE_INIT {
+ init(valueAt0, valueAtInfty, tanAt0);
+ Entity::signalRegistration(gainSOUT);
addCommands();
}
-
-void GainAdaptive::
-init( const double& valueAt0,
- const double& valueAtInfty,
- const double& tanAt0 )
-{
+void GainAdaptive::init(const double &valueAt0, const double &valueAtInfty,
+ const double &tanAt0) {
coeff_a = valueAt0 - valueAtInfty;
- if (0 == coeff_a) { coeff_b = 0; }
- else { coeff_b = tanAt0 / coeff_a; }
+ if (0 == coeff_a) {
+ coeff_b = 0;
+ } else {
+ coeff_b = tanAt0 / coeff_a;
+ }
coeff_c = valueAtInfty;
return;
@@ -140,63 +120,54 @@ init( const double& valueAt0,
* - first, imposing a value gref at position xref: g(xref)=gref.
* In that case, B=-1/xref*log((gref-C)/A):
* gnuplot> A=1; C=.1; xref=.1; gref=.4; B=1/xref*log((gref-C)/A)
- * gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)] A*exp(-B*x)+C, A+C-B*x
- * - second solution: imposing to reach a percentage of raise at a given point. It
- * is more or less the same as before, but with gref := C+p*A, for a given p. In that
- * case, B=-1/xref*log(p)
- * gnuplot> A=1; C=.1; xref=.1; p=.1; B=1/xref*log(p)
- * gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)] A*exp(-B*x)+C, A+C-B*x
+ * gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)] A*exp(-B*x)+C,
+ * A+C-B*x
+ * - second solution: imposing to reach a percentage of raise at a given point.
+ * It is more or less the same as before, but with gref := C+p*A, for a given p.
+ * In that case, B=-1/xref*log(p) gnuplot> A=1; C=.1; xref=.1; p=.1;
+ * B=1/xref*log(p) gnuplot> plot [0:(A+C)/B*10][C-.1*(A-C):A+C+.1*(A-C)]
+ * A*exp(-B*x)+C, A+C-B*x
*
* The second solution is tried in the following.
*/
-void GainAdaptive::
-initFromPassingPoint( const double& valueAt0,
- const double& valueAtInfty,
- const double& xref,
- const double& p ) //gref )
+void GainAdaptive::initFromPassingPoint(const double &valueAt0,
+ const double &valueAtInfty,
+ const double &xref,
+ const double &p) // gref )
{
coeff_c = valueAtInfty;
coeff_a = valueAt0 - valueAtInfty;
- if (0 == coeff_a) { coeff_b = 0; }
- else
- {
- //coeff_b = -1/xref*log( (gref-coeff_c)/coeff_a );
- coeff_b = -1/xref*log( p );
- }
+ if (0 == coeff_a) {
+ coeff_b = 0;
+ } else {
+ // coeff_b = -1/xref*log( (gref-coeff_c)/coeff_a );
+ coeff_b = -1 / xref * log(p);
+ }
}
-
-void GainAdaptive::
-forceConstant( void )
-{
- coeff_a = 0;
-}
+void GainAdaptive::forceConstant(void) { coeff_a = 0; }
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void GainAdaptive::
-display( std::ostream& os ) const
-{
- os << "Gain Adaptative "<<getName();
+void GainAdaptive::display(std::ostream &os) const {
+ os << "Gain Adaptative " << getName();
try {
- os <<" = "<<double(gainSOUT.accessCopy ());
+ os << " = " << double(gainSOUT.accessCopy());
+ } catch (ExceptionSignal e) {
}
- catch (ExceptionSignal e) {}
- os <<" ("<<coeff_a<<";"<<coeff_b<<";"<<coeff_c<<") ";
+ os << " (" << coeff_a << ";" << coeff_b << ";" << coeff_c << ") ";
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-double& GainAdaptive::
-computeGain( double&res, int t )
-{
+double &GainAdaptive::computeGain(double &res, int t) {
sotDEBUGIN(15);
- const dynamicgraph::Vector& error = errorSIN(t);
- const double norm = error.norm();
- res = coeff_a * exp( -coeff_b*norm ) + coeff_c;
+ const dynamicgraph::Vector &error = errorSIN(t);
+ const double norm = error.norm();
+ res = coeff_a * exp(-coeff_b * norm) + coeff_c;
sotDEBUGOUT(15);
return res;
diff --git a/src/task/gain-hyperbolic.cpp b/src/task/gain-hyperbolic.cpp
index bc2b078f..2c8ec221 100644
--- a/src/task/gain-hyperbolic.cpp
+++ b/src/task/gain-hyperbolic.cpp
@@ -14,117 +14,90 @@
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include <sot/core/factory.hh>
#include <sot/core/debug.hh>
#include <sot/core/exception-signal.hh>
+#include <sot/core/factory.hh>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GainHyperbolic,"GainHyperbolic");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GainHyperbolic, "GainHyperbolic");
-const double GainHyperbolic::
-ZERO_DEFAULT = .1;
-const double GainHyperbolic::
-INFTY_DEFAULT = .1;
-const double GainHyperbolic::
-TAN_DEFAULT = 1;
+const double GainHyperbolic::ZERO_DEFAULT = .1;
+const double GainHyperbolic::INFTY_DEFAULT = .1;
+const double GainHyperbolic::TAN_DEFAULT = 1;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+#define __SOT_GAIN_HYPERBOLIC_INIT \
+ Entity(name), coeff_a(0), coeff_b(0), coeff_c(0), coeff_d(0), \
+ errorSIN(NULL, "sotGainHyperbolic(" + name + ")::input(vector)::error"), \
+ gainSOUT(boost::bind(&GainHyperbolic::computeGain, this, _1, _2), \
+ errorSIN, \
+ "sotGainHyperbolic(" + name + ")::output(double)::gain")
-#define __SOT_GAIN_HYPERBOLIC_INIT \
-Entity(name) \
-,coeff_a(0) \
-,coeff_b(0) \
-,coeff_c(0) \
-,coeff_d(0) \
-,errorSIN(NULL,"sotGainHyperbolic("+name+")::input(vector)::error") \
-,gainSOUT( boost::bind(&GainHyperbolic::computeGain,this,_1,_2), \
- errorSIN,"sotGainHyperbolic("+name+")::output(double)::gain" )
-
-
-
-GainHyperbolic::
-GainHyperbolic( const std::string & name )
- :__SOT_GAIN_HYPERBOLIC_INIT
-{
- sotDEBUG(15) << "New gain <"<<name<<">"<<std::endl;
+GainHyperbolic::GainHyperbolic(const std::string &name)
+ : __SOT_GAIN_HYPERBOLIC_INIT {
+ sotDEBUG(15) << "New gain <" << name << ">" << std::endl;
init();
- Entity::signalRegistration( gainSOUT<<errorSIN );
+ Entity::signalRegistration(gainSOUT << errorSIN);
}
-
-GainHyperbolic::
-GainHyperbolic( const std::string & name,const double& lambda )
- :__SOT_GAIN_HYPERBOLIC_INIT
-{
+GainHyperbolic::GainHyperbolic(const std::string &name, const double &lambda)
+ : __SOT_GAIN_HYPERBOLIC_INIT {
init(lambda);
- Entity::signalRegistration( gainSOUT );
+ Entity::signalRegistration(gainSOUT);
}
-GainHyperbolic::
-GainHyperbolic( const std::string & name,
- const double& valueAt0,
- const double& valueAtInfty,
- const double& tanAt0,
- const double& decal0 )
- :__SOT_GAIN_HYPERBOLIC_INIT
-{
- init(valueAt0,valueAtInfty,tanAt0,decal0);
- Entity::signalRegistration( gainSOUT );
+GainHyperbolic::GainHyperbolic(const std::string &name, const double &valueAt0,
+ const double &valueAtInfty, const double &tanAt0,
+ const double &decal0)
+ : __SOT_GAIN_HYPERBOLIC_INIT {
+ init(valueAt0, valueAtInfty, tanAt0, decal0);
+ Entity::signalRegistration(gainSOUT);
}
-
-void GainHyperbolic::
-init( const double& valueAt0,
- const double& valueAtInfty,
- const double& tanAt0,
- const double& decal0 )
-{
+void GainHyperbolic::init(const double &valueAt0, const double &valueAtInfty,
+ const double &tanAt0, const double &decal0) {
coeff_a = valueAt0 - valueAtInfty;
- if (0 == coeff_a) { coeff_b = 0; }
- else { coeff_b = tanAt0 / coeff_a/2; }
+ if (0 == coeff_a) {
+ coeff_b = 0;
+ } else {
+ coeff_b = tanAt0 / coeff_a / 2;
+ }
coeff_c = valueAtInfty;
coeff_d = decal0;
return;
}
-void GainHyperbolic::
-forceConstant( void )
-{
- coeff_a = 0;
-}
+void GainHyperbolic::forceConstant(void) { coeff_a = 0; }
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void GainHyperbolic::
-display( std::ostream& os ) const
-{
- os << "Gain Hyperbolic "<<getName();
+void GainHyperbolic::display(std::ostream &os) const {
+ os << "Gain Hyperbolic " << getName();
try {
- os <<" = "<<double(gainSOUT.accessCopy ());
- } catch (ExceptionSignal e) {}
- //os <<" ("<<coeff_a<<";"<<coeff_b<<";"<<coeff_c<<coeff_d<<") ";
- os <<" ("<<coeff_a<<".exp(-"<<coeff_b<<"(x-" << coeff_d << "))+" <<coeff_c;
+ os << " = " << double(gainSOUT.accessCopy());
+ } catch (ExceptionSignal e) {
+ }
+ // os <<" ("<<coeff_a<<";"<<coeff_b<<";"<<coeff_c<<coeff_d<<") ";
+ os << " (" << coeff_a << ".exp(-" << coeff_b << "(x-" << coeff_d << "))+"
+ << coeff_c;
}
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-double& GainHyperbolic::
-computeGain( double&res, int t )
-{
+double &GainHyperbolic::computeGain(double &res, int t) {
sotDEBUGIN(15);
- const dynamicgraph::Vector& error = errorSIN(t);
- const double norm = error.norm();
- res = coeff_a *.5* (tanh( -coeff_b*(norm-coeff_d) )+1 ) + coeff_c;
+ const dynamicgraph::Vector &error = errorSIN(t);
+ const double norm = error.norm();
+ res = coeff_a * .5 * (tanh(-coeff_b * (norm - coeff_d)) + 1) + coeff_c;
sotDEBUGOUT(15);
return res;
diff --git a/src/task/multi-bound.cpp b/src/task/multi-bound.cpp
index 8246a8a9..2590b4e8 100644
--- a/src/task/multi-bound.cpp
+++ b/src/task/multi-bound.cpp
@@ -13,249 +13,237 @@
using namespace dynamicgraph::sot;
-
-MultiBound::
-MultiBound( const double x )
- : 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) {}
-
-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),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) {}
-
-MultiBound::MultiBoundModeType MultiBound::
-getMode( void ) const
-{ return mode; }
-double MultiBound::
-getSingleBound( void ) const
-{
- if( MODE_SINGLE!=mode )
- {
- SOT_THROW ExceptionTask( ExceptionTask::BOUND_TYPE,
- "Accessing single bound of a non-single type.");
- }
+MultiBound::MultiBound(const double x)
+ : 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) {}
+
+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),
+ 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) {}
+
+MultiBound::MultiBoundModeType MultiBound::getMode(void) const { return mode; }
+double MultiBound::getSingleBound(void) const {
+ if (MODE_SINGLE != mode) {
+ SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
+ "Accessing single bound of a non-single type.");
+ }
return boundSingle;
}
-double MultiBound::
-getDoubleBound( const MultiBound::SupInfType bound ) const
-{
- if( MODE_DOUBLE!=mode )
- {
- SOT_THROW ExceptionTask( ExceptionTask::BOUND_TYPE,
- "Accessing double bound of a non-double type.");
+double MultiBound::getDoubleBound(const MultiBound::SupInfType bound) const {
+ if (MODE_DOUBLE != mode) {
+ SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
+ "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.");
}
- switch(bound)
- {
- case BOUND_SUP:
- {
- if(! boundSupSetup)
- {SOT_THROW ExceptionTask( ExceptionTask::BOUND_TYPE,
- "Accessing un-setup sup bound.");}
- return boundSup;
- }
- case BOUND_INF:
- {
- if(! boundInfSetup)
- {SOT_THROW ExceptionTask( ExceptionTask::BOUND_TYPE,
- "Accessing un-setup inf bound");}
- return boundInf;
- }
+ return boundSup;
+ }
+ case BOUND_INF: {
+ if (!boundInfSetup) {
+ SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
+ "Accessing un-setup inf bound");
}
+ return boundInf;
+ }
+ }
return 0;
}
-bool MultiBound::
-getDoubleBoundSetup( const MultiBound::SupInfType bound ) const
-{
- if( MODE_DOUBLE!=mode )
- {
- SOT_THROW ExceptionTask( ExceptionTask::BOUND_TYPE,
- "Accessing double bound of a non-double type.");
- }
- switch(bound)
- {
- case BOUND_SUP:
- return boundSupSetup;
- case BOUND_INF:
- return boundInfSetup;
- }
+bool MultiBound::getDoubleBoundSetup(const MultiBound::SupInfType bound) const {
+ if (MODE_DOUBLE != mode) {
+ SOT_THROW ExceptionTask(ExceptionTask::BOUND_TYPE,
+ "Accessing double bound of a non-double type.");
+ }
+ switch (bound) {
+ case BOUND_SUP:
+ return boundSupSetup;
+ case BOUND_INF:
+ return boundInfSetup;
+ }
return false;
}
-void MultiBound::
-setDoubleBound( SupInfType boundType,double boundValue )
-{
- if(MODE_DOUBLE!=mode)
- { mode=MODE_DOUBLE; boundSupSetup=false; boundInfSetup=false; }
- switch(boundType)
- {
+void MultiBound::setDoubleBound(SupInfType boundType, double boundValue) {
+ if (MODE_DOUBLE != mode) {
+ mode = MODE_DOUBLE;
+ boundSupSetup = false;
+ boundInfSetup = false;
+ }
+ switch (boundType) {
+ case BOUND_INF:
+ boundInfSetup = true;
+ boundInf = boundValue;
+ break;
+ case BOUND_SUP:
+ boundSupSetup = true;
+ boundSup = boundValue;
+ break;
+ }
+}
+void MultiBound::unsetDoubleBound(SupInfType boundType) {
+ if (MODE_DOUBLE != mode) {
+ mode = MODE_DOUBLE;
+ boundSupSetup = false;
+ boundInfSetup = false;
+ } else {
+ switch (boundType) {
case BOUND_INF:
- boundInfSetup=true; boundInf=boundValue;
+ boundInfSetup = false;
break;
case BOUND_SUP:
- boundSupSetup=true; boundSup=boundValue;
+ boundSupSetup = false;
break;
}
+ }
}
-void MultiBound::
-unsetDoubleBound( SupInfType boundType )
-{
- if(MODE_DOUBLE!=mode)
- { mode=MODE_DOUBLE; boundSupSetup=false; boundInfSetup=false; }
- else
- {
- switch(boundType)
- {
- case BOUND_INF:
- boundInfSetup=false;
- break;
- case BOUND_SUP:
- boundSupSetup=false;
- break;
- }
- }
-}
-void MultiBound::
-setSingleBound( double boundValue )
-{
- mode=MODE_SINGLE;
- boundSingle=boundValue;
+void MultiBound::setSingleBound(double boundValue) {
+ mode = MODE_SINGLE;
+ boundSingle = boundValue;
}
-inline static void SOT_MULTI_BOUND_CHECK_C(std::istream& is,
- char check,
- VectorMultiBound& v)
-{
+inline static void SOT_MULTI_BOUND_CHECK_C(std::istream &is, char check,
+ VectorMultiBound &v) {
char c;
is.get(c);
- if(c!=check)
- {
- v.resize(0);
- sotERROR << "Error while parsing vector multi-bound. Waiting for a '" << check
- << "'. Get '" << c << "' instead. " << std::endl;
- SOT_THROW ExceptionTask(ExceptionTask::PARSER_MULTI_BOUND,
- "Error parsing vector multi-bound.");
- }
+ if (c != check) {
+ v.resize(0);
+ sotERROR << "Error while parsing vector multi-bound. Waiting for a '"
+ << check << "'. Get '" << c << "' instead. " << std::endl;
+ SOT_THROW ExceptionTask(ExceptionTask::PARSER_MULTI_BOUND,
+ "Error parsing vector multi-bound.");
+ }
}
-namespace dynamicgraph { 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;
- }
- }
+namespace dynamicgraph {
+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;
+ }
+ }
return os;
}
-std::istream& operator>> ( std::istream& is, MultiBound & m )
-{
+std::istream &operator>>(std::istream &is, MultiBound &m) {
sotDEBUGIN(15);
- char c; double val;
+ char c;
+ double val;
is.get(c);
- if( c=='(' )
- {
- sotDEBUG(15) << "Double" << std::endl;
- char c2[3];
- is.get(c2,3);
- if( std::string(c2,2)!="--" )
- {
- is.putback(c2[1]); is.putback(c2[0]);
- //{char strbuf[256]; is.getline(strbuf,256); sotDEBUG(1) << "#"<<strbuf<<"#"<<std::endl;}
- is>>val;
- sotDEBUG(15) << "First val = " << val << std::endl;
- m.setDoubleBound(MultiBound::BOUND_INF,val);
- } else { m.unsetDoubleBound(MultiBound::BOUND_INF); }
- is.get(c);
- if( c!=',' )
- {
- sotERROR << "Error while parsing multi-bound. Waiting for a ','. Get '"
- << c << "' instead. " << std::endl;
- SOT_THROW ExceptionTask(ExceptionTask::PARSER_MULTI_BOUND,
- "Error parsing multi-bound, while waiting for a ','.");
- }
-
- is.get(c2,3);
- if( std::string(c2,2)!="--" )
- {
- is.putback(c2[1]); is.putback(c2[0]);
- is>>val;
- sotDEBUG(15) << "Second val = " << val << std::endl;
- m.setDoubleBound(MultiBound::BOUND_SUP,val);
- } else { m.unsetDoubleBound(MultiBound::BOUND_SUP); }
- is.get(c);
- if( c!=')' )
- {
- sotERROR << "Error while parsing multi-bound. Waiting for a ')'. Get '"
- << c << "' instead. " << std::endl;
- SOT_THROW ExceptionTask(ExceptionTask::PARSER_MULTI_BOUND,
- "Error parsing multi-bound, while waiting for a ')'.");
- }
+ if (c == '(') {
+ sotDEBUG(15) << "Double" << std::endl;
+ char c2[3];
+ is.get(c2, 3);
+ if (std::string(c2, 2) != "--") {
+ is.putback(c2[1]);
+ is.putback(c2[0]);
+ //{char strbuf[256]; is.getline(strbuf,256); sotDEBUG(1) <<
+ //"#"<<strbuf<<"#"<<std::endl;}
+ is >> val;
+ sotDEBUG(15) << "First val = " << val << std::endl;
+ m.setDoubleBound(MultiBound::BOUND_INF, val);
+ } else {
+ m.unsetDoubleBound(MultiBound::BOUND_INF);
}
- else
- {
- sotDEBUG(15) << "Single ('" << c << "')"<<std::endl;
- is.putback(c);
+ is.get(c);
+ if (c != ',') {
+ sotERROR << "Error while parsing multi-bound. Waiting for a ','. Get '"
+ << c << "' instead. " << std::endl;
+ SOT_THROW ExceptionTask(
+ ExceptionTask::PARSER_MULTI_BOUND,
+ "Error parsing multi-bound, while waiting for a ','.");
+ }
+
+ is.get(c2, 3);
+ if (std::string(c2, 2) != "--") {
+ is.putback(c2[1]);
+ is.putback(c2[0]);
is >> val;
- m.setSingleBound(val);
+ sotDEBUG(15) << "Second val = " << val << std::endl;
+ m.setDoubleBound(MultiBound::BOUND_SUP, val);
+ } else {
+ m.unsetDoubleBound(MultiBound::BOUND_SUP);
+ }
+ is.get(c);
+ if (c != ')') {
+ sotERROR << "Error while parsing multi-bound. Waiting for a ')'. Get '"
+ << c << "' instead. " << std::endl;
+ SOT_THROW ExceptionTask(
+ ExceptionTask::PARSER_MULTI_BOUND,
+ "Error parsing multi-bound, while waiting for a ')'.");
}
+ } else {
+ sotDEBUG(15) << "Single ('" << c << "')" << std::endl;
+ is.putback(c);
+ is >> val;
+ m.setSingleBound(val);
+ }
sotDEBUGOUT(15);
return is;
}
-std::ostream& operator<< (std::ostream& os, const VectorMultiBound& v )
-{
+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<<","; os << (*iter); }
- return os<<")";
+ for (VectorMultiBound::const_iterator iter = v.begin(); iter != v.end();
+ ++iter) {
+ if (iter != v.begin())
+ os << ",";
+ os << (*iter);
+ }
+ return os << ")";
}
-std::istream& operator>> (std::istream& is, VectorMultiBound& v )
-{
+std::istream &operator>>(std::istream &is, VectorMultiBound &v) {
unsigned int vali;
/* Read the vector size. */
- SOT_MULTI_BOUND_CHECK_C(is,'[',v);
- is>>vali;
+ SOT_MULTI_BOUND_CHECK_C(is, '[', v);
+ is >> vali;
v.resize(vali);
- SOT_MULTI_BOUND_CHECK_C(is,']',v);
+ SOT_MULTI_BOUND_CHECK_C(is, ']', v);
/* Loop for the vals. */
- SOT_MULTI_BOUND_CHECK_C(is,'(',v);
- for( unsigned int i=0;i<vali;++i )
- {
- is>>v[i];
- if( i!=vali-1 ) { SOT_MULTI_BOUND_CHECK_C(is,',',v); }
- else { SOT_MULTI_BOUND_CHECK_C(is,')',v); }
+ SOT_MULTI_BOUND_CHECK_C(is, '(', v);
+ for (unsigned int i = 0; i < vali; ++i) {
+ is >> v[i];
+ if (i != vali - 1) {
+ SOT_MULTI_BOUND_CHECK_C(is, ',', v);
+ } else {
+ SOT_MULTI_BOUND_CHECK_C(is, ')', v);
}
+ }
return is;
}
-} /* namespace sot */} /* namespace dynamicgraph */
+} /* namespace sot */
+} /* namespace dynamicgraph */
diff --git a/src/task/task-abstract.cpp b/src/task/task-abstract.cpp
index c976e00a..2c9fafd1 100644
--- a/src/task/task-abstract.cpp
+++ b/src/task/task-abstract.cpp
@@ -12,32 +12,24 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/task-abstract.hh>
#include <sot/core/pool.hh>
+#include <sot/core/task-abstract.hh>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-TaskAbstract::
-TaskAbstract( const std::string& n )
- :Entity(n)
- ,memoryInternal(NULL)
- ,taskSOUT( "sotTaskAbstract("+n+")::output(vector)::task" )
- ,jacobianSOUT( "sotTaskAbstract("+n+")::output(matrix)::jacobian" )
-{
+TaskAbstract::TaskAbstract(const std::string &n)
+ : Entity(n), memoryInternal(NULL),
+ taskSOUT("sotTaskAbstract(" + n + ")::output(vector)::task"),
+ jacobianSOUT("sotTaskAbstract(" + n + ")::output(matrix)::jacobian") {
taskRegistration();
- signalRegistration( taskSOUT<<jacobianSOUT );
+ signalRegistration(taskSOUT << jacobianSOUT);
}
-
-void TaskAbstract::
-taskRegistration( void )
-{
- PoolStorage::getInstance()->registerTask(name,this);
+void TaskAbstract::taskRegistration(void) {
+ PoolStorage::getInstance()->registerTask(name, this);
}
diff --git a/src/task/task-command.h b/src/task/task-command.h
index f9d5df00..04704c07 100644
--- a/src/task/task-command.h
+++ b/src/task/task-command.h
@@ -7,48 +7,45 @@
*/
#ifndef TASK_COMMAND_H
- #define TASK_COMMAND_H
+#define TASK_COMMAND_H
- #include <boost/assign/list_of.hpp>
+#include <boost/assign/list_of.hpp>
- #include <dynamic-graph/command.h>
- #include <dynamic-graph/command-setter.h>
- #include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command.h>
-namespace dynamicgraph { namespace sot {
- namespace command {
- namespace task {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
+namespace dynamicgraph {
+namespace sot {
+namespace command {
+namespace task {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
- // Command ListFeatures
- class ListFeatures : public Command
- {
- public:
- virtual ~ListFeatures() {}
- /// Create command and store it in Entity
- /// \param entity instance of Entity owning this command
- /// \param docstring documentation of the command
- ListFeatures(Task& entity, const std::string& docstring) :
- Command(entity, std::vector<Value::Type> (), docstring)
- {
- }
- virtual Value doExecute()
- {
- typedef Task::FeatureList_t FeatureList_t;
- Task& task = static_cast<Task&>(owner());
- const FeatureList_t& fl = task.getFeatureList();
- std::string result("[");
- for (FeatureList_t::const_iterator it = fl.begin ();
- it != fl.end (); it++) {
- result += "'" + (*it)->getName() + "',";
- }
- result += "]";
- return Value(result);
- }
- }; // class ListFeatures
- } // namespace task
- } // namespace command
-} /* namespace sot */} /* namespace dynamicgraph */
+// Command ListFeatures
+class ListFeatures : public Command {
+public:
+ virtual ~ListFeatures() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ ListFeatures(Task &entity, const std::string &docstring)
+ : Command(entity, std::vector<Value::Type>(), docstring) {}
+ virtual Value doExecute() {
+ typedef Task::FeatureList_t FeatureList_t;
+ Task &task = static_cast<Task &>(owner());
+ const FeatureList_t &fl = task.getFeatureList();
+ std::string result("[");
+ for (FeatureList_t::const_iterator it = fl.begin(); it != fl.end(); it++) {
+ result += "'" + (*it)->getName() + "',";
+ }
+ result += "]";
+ return Value(result);
+ }
+}; // 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 a5488911..a586aebb 100644
--- a/src/task/task-conti.cpp
+++ b/src/task/task-conti.cpp
@@ -12,92 +12,89 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/task-conti.hh>
-#include <sot/core/debug.hh>
#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
+#include <sot/core/task-conti.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskConti,"TaskConti");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskConti, "TaskConti");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-TaskConti::
-TaskConti( const std::string& n )
- :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 );
+TaskConti::TaskConti(const std::string &n)
+ : 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);
- const double & lambda = controlGainSIN(timecurr);
+ const double &lambda = controlGainSIN(timecurr);
- try{
- const dynamicgraph::Matrix & J = jacobianSOUT(timecurr);
+ try {
+ const dynamicgraph::Matrix &J = jacobianSOUT(timecurr);
- dynamicgraph::Vector deref( J.rows() );
+ 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
- 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);
- }
-
+ 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);
+ }
- if( timeRef==TIME_REF_TO_BE_SET ) { timeRef = timecurr; }
- if( timeRef<0 ) { sotDEBUG(10) << "Time not used. " << std::endl; throw 1;}
+ if (timeRef == TIME_REF_TO_BE_SET) {
+ timeRef = timecurr;
+ }
+ if (timeRef < 0) {
+ sotDEBUG(10) << "Time not used. " << std::endl;
+ throw 1;
+ }
- double dt = timeRef-timecurr; dt*=mu/200.0;
+ double dt = timeRef - timecurr;
+ dt *= mu / 200.0;
double contiGain = exp(dt);
- double gain = (contiGain-1)*lambda;
+ double gain = (contiGain - 1) * lambda;
sotDEBUG(25) << "T: ref=" << timeRef << ", cur=" << timecurr << std::endl;
- sotDEBUG(25) << "Gains: l=" << lambda << ", expmu=" << contiGain << std::endl;
- sotDEBUG(25) << "e = " << deref<<std::endl;
+ sotDEBUG(25) << "Gains: l=" << lambda << ", expmu=" << contiGain
+ << std::endl;
+ sotDEBUG(25) << "e = " << deref << std::endl;
desvel *= gain;
- sotDEBUG(25) << "dedes: " << desvel <<std::endl;
+ sotDEBUG(25) << "dedes: " << desvel << std::endl;
deref *= contiGain;
desvel += deref;
- sotDEBUG(25) << "task: " << desvel <<std::endl;
+ sotDEBUG(25) << "task: " << desvel << std::endl;
desvel2b.resize(desvel.size());
- for( int i=0;i<desvel.size(); ++i )
+ for (int i = 0; i < desvel.size(); ++i)
desvel2b[i] = desvel(i);
-
sotDEBUG(15) << "# Out }" << endl;
return desvel2b;
- } catch (...)
- {
- 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);
- return desvel2b;
- }
-
+ } catch (...) {
+ 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);
+ return desvel2b;
+ }
}
/* --- COMPUTATION ---------------------------------------------------------- */
@@ -108,18 +105,13 @@ computeContiDesiredVelocity( VectorMultiBound& desvel2b,const int & timecurr )
/* --- DISPLAY ------------------------------------------------------------ */
/* --- DISPLAY ------------------------------------------------------------ */
-void TaskConti::
-display( std::ostream& os ) const
-{
- os << "TaskConti " << name
- << " [t=" << timeRef << "] "
+void TaskConti::display(std::ostream &os) const {
+ os << "TaskConti " << name << " [t=" << timeRef << "] "
<< ": " << endl;
os << "--- LIST --- " << std::endl;
- for( std::list< FeatureAbstract* >::const_iterator iter = featureList.begin();
- iter!=featureList.end(); ++iter )
- {
- os << "-> " << (*iter)->getName() <<endl;
- }
-
+ for (std::list<FeatureAbstract *>::const_iterator iter = featureList.begin();
+ iter != featureList.end(); ++iter) {
+ os << "-> " << (*iter)->getName() << endl;
+ }
}
diff --git a/src/task/task-pd.cpp b/src/task/task-pd.cpp
index 824a8fcb..b47fcdf2 100644
--- a/src/task/task-pd.cpp
+++ b/src/task/task-pd.cpp
@@ -12,101 +12,82 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/task-pd.hh>
-#include <sot/core/debug.hh>
#include <dynamic-graph/all-commands.h>
+#include <sot/core/debug.hh>
+#include <sot/core/task-pd.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
-
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskPD,"TaskPD");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskPD, "TaskPD");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+TaskPD::TaskPD(const std::string &n)
+ : Task(n), previousError(), beta(1),
+ errorDotSOUT(boost::bind(&TaskPD::computeErrorDot, this, _1, _2),
+ errorSOUT,
+ "sotTaskPD(" + n + ")::output(vector)::errorDotOUT"),
+ errorDotSIN(NULL, "sotTaskPD(" + n + ")::input(vector)::errorDot") {
+ taskSOUT.setFunction(boost::bind(&TaskPD::computeTaskModif, this, _1, _2));
+ taskSOUT.addDependency(errorDotSOUT);
-TaskPD::
-TaskPD( const std::string& n )
- :Task(n)
- ,previousError()
- ,beta(1)
- ,errorDotSOUT( boost::bind(&TaskPD::computeErrorDot,this,_1,_2),
- errorSOUT,
- "sotTaskPD("+n+")::output(vector)::errorDotOUT" )
- ,errorDotSIN( NULL,
- "sotTaskPD("+n+")::input(vector)::errorDot" )
-{
- taskSOUT.setFunction( boost::bind(&TaskPD::computeTaskModif,this,_1,_2) );
- taskSOUT.addDependency( errorDotSOUT );
-
- signalRegistration( errorDotSOUT<<errorDotSIN );
+ signalRegistration(errorDotSOUT << errorDotSIN);
initCommand();
- errorDotSIN.plug( &errorDotSOUT );
+ errorDotSIN.plug(&errorDotSOUT);
}
-
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
-dynamicgraph::Vector& TaskPD::
-computeErrorDot( dynamicgraph::Vector& errorDot,int time )
-{
+dynamicgraph::Vector &TaskPD::computeErrorDot(dynamicgraph::Vector &errorDot,
+ int time) {
sotDEBUG(15) << "# In {" << endl;
- const dynamicgraph::Vector & errCur = errorSOUT(time);
- if( previousError.size() == errCur.size() )
- {
- errorDot = errCur;
- errorDot -= previousError;
- previousError = errCur;
- }
- else
- {
- errorDot.resize( errCur.size() );
- errorDot.setZero();
- previousError = errCur;
- }
+ const dynamicgraph::Vector &errCur = errorSOUT(time);
+ if (previousError.size() == errCur.size()) {
+ errorDot = errCur;
+ errorDot -= previousError;
+ previousError = errCur;
+ } else {
+ errorDot.resize(errCur.size());
+ errorDot.setZero();
+ previousError = errCur;
+ }
sotDEBUG(15) << "# Out }" << endl;
return errorDot;
}
-VectorMultiBound& TaskPD::
-computeTaskModif( VectorMultiBound& task,int time )
-{
+VectorMultiBound &TaskPD::computeTaskModif(VectorMultiBound &task, int time) {
sotDEBUG(15) << "# In {" << endl;
- const dynamicgraph::Vector & errorDot = errorDotSIN(time);
- Task::computeTaskExponentialDecrease(task,time);
+ const dynamicgraph::Vector &errorDot = errorDotSIN(time);
+ Task::computeTaskExponentialDecrease(task, time);
sotDEBUG(25) << " Task = " << task;
sotDEBUG(25) << " edot = " << errorDot;
- for( unsigned int i=0;i<task.size(); ++i )
- { task[i] = task[i].getSingleBound()-( beta * errorDot(i) ); }
+ for (unsigned int i = 0; i < task.size(); ++i) {
+ task[i] = task[i].getSingleBound() - (beta * errorDot(i));
+ }
sotDEBUG(15) << "# Out }" << endl;
return task;
}
-
-
-
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
#include <sot/core/pool.hh>
-void TaskPD::
-initCommand( void )
-{
+void TaskPD::initCommand(void) {
using namespace command;
- addCommand("setBeta",makeDirectSetter(*this,&beta,docDirectSetter("beta","double")));
+ addCommand("setBeta",
+ makeDirectSetter(*this, &beta, docDirectSetter("beta", "double")));
}
diff --git a/src/task/task-unilateral.cpp b/src/task/task-unilateral.cpp
index cb143a4d..e8cf9f8f 100644
--- a/src/task/task-unilateral.cpp
+++ b/src/task/task-unilateral.cpp
@@ -15,64 +15,61 @@
//#define VP_DEBUG_MODE 15
/* SOT */
-#include <sot/core/task-unilateral.hh>
#include <sot/core/debug.hh>
+#include <sot/core/task-unilateral.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskUnilateral,"TaskUnilateral");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TaskUnilateral, "TaskUnilateral");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-TaskUnilateral::
-TaskUnilateral( const std::string& n )
- :Task(n)
- ,featureList()
- ,positionSIN( NULL,"sotTaskUnilateral("+n+")::input(vector)::position" )
- ,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) );
+TaskUnilateral::TaskUnilateral(const std::string &n)
+ : Task(n), featureList(),
+ positionSIN(NULL,
+ "sotTaskUnilateral(" + n + ")::input(vector)::position"),
+ 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));
taskSOUT.clearDependencies();
- taskSOUT.addDependency( referenceSupSIN );
- taskSOUT.addDependency( referenceInfSIN );
- taskSOUT.addDependency( dtSIN );
- taskSOUT.addDependency( positionSIN );
+ taskSOUT.addDependency(referenceSupSIN);
+ taskSOUT.addDependency(referenceInfSIN);
+ taskSOUT.addDependency(dtSIN);
+ taskSOUT.addDependency(positionSIN);
- signalRegistration( referenceSupSIN<<dtSIN<<referenceInfSIN<<positionSIN );
+ signalRegistration(referenceSupSIN << dtSIN << referenceInfSIN
+ << positionSIN);
}
-
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
-VectorMultiBound& TaskUnilateral::
-computeTaskUnilateral( VectorMultiBound& res,int time )
-{
+VectorMultiBound &TaskUnilateral::computeTaskUnilateral(VectorMultiBound &res,
+ int time) {
sotDEBUG(45) << "# In " << getName() << " {" << endl;
- const dynamicgraph::Vector & position = positionSIN(time);
+ const dynamicgraph::Vector &position = positionSIN(time);
sotDEBUG(35) << "position = " << position << endl;
- const dynamicgraph::Vector & refInf = referenceInfSIN(time);
- const dynamicgraph::Vector & refSup = referenceSupSIN(time);
- const double & dt = dtSIN(time);
+ const dynamicgraph::Vector &refInf = referenceInfSIN(time);
+ const dynamicgraph::Vector &refSup = referenceSupSIN(time);
+ const double &dt = dtSIN(time);
res.resize(position.size());
- for( unsigned int i=0;i<res.size();++i )
- {
- MultiBound toto((refInf(i)-position(i))/dt,(refSup(i)-position(i))/dt);
- res[i] = toto;
- }
+ for (unsigned int i = 0; i < res.size(); ++i) {
+ MultiBound toto((refInf(i) - position(i)) / dt,
+ (refSup(i) - position(i)) / dt);
+ res[i] = toto;
+ }
- sotDEBUG(15) << "taskU = "<< res << std::endl;
+ sotDEBUG(15) << "taskU = " << res << std::endl;
sotDEBUG(45) << "# Out }" << endl;
return res;
}
@@ -81,8 +78,6 @@ computeTaskUnilateral( VectorMultiBound& res,int time )
/* --- DISPLAY ------------------------------------------------------------ */
/* --- DISPLAY ------------------------------------------------------------ */
-void TaskUnilateral::
-display( std::ostream& os ) const
-{
+void TaskUnilateral::display(std::ostream &os) const {
os << "TaskUnilateral " << name << ": " << endl;
}
diff --git a/src/task/task.cpp b/src/task/task.cpp
index 5469641d..34ffd63f 100644
--- a/src/task/task.cpp
+++ b/src/task/task.cpp
@@ -12,62 +12,57 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/task.hh>
#include <sot/core/debug.hh>
+#include <sot/core/task.hh>
-#include <sot/core/pool.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;
using namespace dynamicgraph;
#include <sot/core/factory.hh>
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Task,"Task");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Task, "Task");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-Task::
-Task( const std::string& n )
- :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.
- ,controlSelectionSIN( NULL,"sotTask("+n+")::input(flag)::controlSelec" )
- ,errorSOUT( boost::bind(&Task::computeError,this,_1,_2),
- sotNOSIGNAL,
- "sotTask("+n+")::output(vector)::error" )
- ,errorTimeDerivativeSOUT( boost::bind(&Task::computeErrorTimeDerivative,this,_1,_2),
- errorSOUT,
- "sotTask("+n+")::output(vector)::errorTimeDerivative" )
-{
- taskSOUT.setFunction( boost::bind(&Task::computeTaskExponentialDecrease,this,_1,_2) );
- jacobianSOUT.setFunction( boost::bind(&Task::computeJacobian,this,_1,_2) );
-
- taskSOUT.addDependency( controlGainSIN );
- taskSOUT.addDependency( errorSOUT );
- taskSOUT.addDependency( errorTimeDerivativeSOUT );
-
- jacobianSOUT.addDependency( controlSelectionSIN );
+Task::Task(const std::string &n)
+ : 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.
+ ,
+ controlSelectionSIN(NULL,
+ "sotTask(" + n + ")::input(flag)::controlSelec"),
+ errorSOUT(boost::bind(&Task::computeError, this, _1, _2), sotNOSIGNAL,
+ "sotTask(" + n + ")::output(vector)::error"),
+ errorTimeDerivativeSOUT(
+ boost::bind(&Task::computeErrorTimeDerivative, this, _1, _2),
+ errorSOUT,
+ "sotTask(" + n + ")::output(vector)::errorTimeDerivative") {
+ taskSOUT.setFunction(
+ boost::bind(&Task::computeTaskExponentialDecrease, this, _1, _2));
+ jacobianSOUT.setFunction(boost::bind(&Task::computeJacobian, this, _1, _2));
+
+ taskSOUT.addDependency(controlGainSIN);
+ taskSOUT.addDependency(errorSOUT);
+ taskSOUT.addDependency(errorTimeDerivativeSOUT);
+
+ jacobianSOUT.addDependency(controlSelectionSIN);
controlSelectionSIN = true;
- signalRegistration( controlGainSIN<<dampingGainSINOUT
- <<controlSelectionSIN<<errorSOUT<<errorTimeDerivativeSOUT );
-
+ signalRegistration(controlGainSIN << dampingGainSINOUT << controlSelectionSIN
+ << errorSOUT << errorTimeDerivativeSOUT);
initCommands();
}
-void Task::initCommands( void )
-{
+void Task::initCommands(void) {
using namespace dynamicgraph::command;
//
// Commands
@@ -75,108 +70,83 @@ 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";
+ " Add a feature to the task\n"
+ " \n"
+ " Input:\n"
+ " - name of the feature\n"
+ " \n";
addCommand("add",
- makeCommandVoid1(*this,&Task::addFeatureFromName,docstring));
+ makeCommandVoid1(*this, &Task::addFeatureFromName, docstring));
addCommand("setWithDerivative",
- makeDirectSetter(*this,&withDerivative,
- docDirectSetter("withDerivative","bool")));
+ makeDirectSetter(*this, &withDerivative,
+ docDirectSetter("withDerivative", "bool")));
addCommand("getWithDerivative",
- makeDirectGetter(*this,&withDerivative,
- docDirectGetter("withDerivative","bool")));
+ makeDirectGetter(*this, &withDerivative,
+ docDirectGetter("withDerivative", "bool")));
// ClearFeatureList
docstring = " \n"
- " Clear the list of features of the task\n"
- " \n";
+ " Clear the list of features of the task\n"
+ " \n";
addCommand("clear",
- makeCommandVoid0(*this,&Task::clearFeatureList, docstring));
+ makeCommandVoid0(*this, &Task::clearFeatureList, docstring));
// List features
docstring = " \n"
- " Returns the list of features of the task\n"
- " \n";
+ " Returns the list of features of the task\n"
+ " \n";
- addCommand("list",
- new command::task::ListFeatures (*this, docstring));
+ addCommand("list", new command::task::ListFeatures(*this, docstring));
}
-
-
-void Task::
-addFeature( FeatureAbstract& s )
-{
+void Task::addFeature(FeatureAbstract &s) {
featureList.push_back(&s);
- jacobianSOUT.addDependency( s.jacobianSOUT );
- errorSOUT.addDependency( s.errorSOUT );
- errorTimeDerivativeSOUT.addDependency (s.getErrorDot());
+ jacobianSOUT.addDependency(s.jacobianSOUT);
+ errorSOUT.addDependency(s.errorSOUT);
+ errorTimeDerivativeSOUT.addDependency(s.getErrorDot());
}
-void Task::
-addFeatureFromName( const std::string & featureName )
-{
- FeatureAbstract& feature =
- PoolStorage::getInstance()->getFeature(featureName);
+void Task::addFeatureFromName(const std::string &featureName) {
+ FeatureAbstract &feature =
+ PoolStorage::getInstance()->getFeature(featureName);
addFeature(feature);
}
-void Task::
-clearFeatureList( void )
-{
-
- for( std::list< FeatureAbstract* >::iterator iter = featureList.begin();
- iter!=featureList.end(); ++iter )
- {
- FeatureAbstract & s = **iter;
- jacobianSOUT.removeDependency( s.jacobianSOUT );
- errorSOUT.removeDependency( s.errorSOUT );
- errorTimeDerivativeSOUT.removeDependency (s.getErrorDot());
- }
+void Task::clearFeatureList(void) {
+
+ for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
+ iter != featureList.end(); ++iter) {
+ FeatureAbstract &s = **iter;
+ jacobianSOUT.removeDependency(s.jacobianSOUT);
+ errorSOUT.removeDependency(s.errorSOUT);
+ errorTimeDerivativeSOUT.removeDependency(s.getErrorDot());
+ }
featureList.clear();
}
-void Task::
-setControlSelection( const Flags& act )
-{
- controlSelectionSIN = act;
-}
-void Task::
-addControlSelection( const Flags& act )
-{
+void Task::setControlSelection(const Flags &act) { controlSelectionSIN = act; }
+void Task::addControlSelection(const Flags &act) {
Flags fl = controlSelectionSIN.accessCopy();
fl &= act;
controlSelectionSIN = fl;
}
-void Task::
-clearControlSelection( void )
-{
- controlSelectionSIN = Flags(false);
-}
+void Task::clearControlSelection(void) { controlSelectionSIN = Flags(false); }
-void Task::
-setWithDerivative( const bool & s )
-{ withDerivative = s; }
-bool Task::
-getWithDerivative( void )
-{ return withDerivative; }
+void Task::setWithDerivative(const bool &s) { withDerivative = s; }
+bool Task::getWithDerivative(void) { return withDerivative; }
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
/* --- COMPUTATION ---------------------------------------------------------- */
-dynamicgraph::Vector& Task::
-computeError( dynamicgraph::Vector& error,int time )
-{
+dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error,
+ int time) {
sotDEBUG(15) << "# In " << getName() << " {" << endl;
- if( featureList.empty())
- { throw( ExceptionTask(ExceptionTask::EMPTY_LIST,
- "Empty feature list") ) ; }
+ if (featureList.empty()) {
+ throw(ExceptionTask(ExceptionTask::EMPTY_LIST, "Empty feature list"));
+ }
try {
/* The vector dimensions are not known before the affectation loop.
@@ -193,138 +163,146 @@ computeError( dynamicgraph::Vector& error,int time )
/* First assumption: vector dimensions have not changed. If 0, they are
* initialized to dim 1.*/
- dynamicgraph::Vector::Index dimError = error .size();
- if( 0==dimError ){ dimError = 1; error.resize(dimError); error.setZero();}
+ dynamicgraph::Vector::Index dimError = error.size();
+ if (0 == dimError) {
+ dimError = 1;
+ error.resize(dimError);
+ error.setZero();
+ }
dynamicgraph::Vector vectTmp;
int cursorError = 0;
/* For each cell of the list, recopy value of s, s_star and error. */
- for( std::list< FeatureAbstract* >::iterator iter = featureList.begin();
- iter!=featureList.end(); ++iter )
- {
- FeatureAbstract &feature = **iter;
+ for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
+ iter != featureList.end(); ++iter) {
+ FeatureAbstract &feature = **iter;
- /* Get s, and store it in the s vector. */
- sotDEBUG(45) << "Feature <" << feature.getName() << ">." << std::endl;
- const dynamicgraph::Vector& partialError = feature.errorSOUT(time);
+ /* Get s, and store it in the s vector. */
+ sotDEBUG(45) << "Feature <" << feature.getName() << ">." << std::endl;
+ const dynamicgraph::Vector &partialError = feature.errorSOUT(time);
- const dynamicgraph::Vector::Index dim = partialError.size();
- while( cursorError+dim>dimError ) // DEBUG It was >=
- { dimError *= 2; error.resize(dimError); error.setZero(); }
+ const dynamicgraph::Vector::Index dim = partialError.size();
+ while (cursorError + dim > dimError) // DEBUG It was >=
+ {
+ dimError *= 2;
+ error.resize(dimError);
+ error.setZero();
+ }
- for( int k=0;k<dim;++k ){ error(cursorError++) = partialError(k); }
- sotDEBUG(35) << "feature: "<< partialError << std::endl;
- sotDEBUG(35) << "error: "<< error << std::endl;
+ for (int k = 0; k < dim; ++k) {
+ error(cursorError++) = partialError(k);
}
+ sotDEBUG(35) << "feature: " << partialError << std::endl;
+ sotDEBUG(35) << "error: " << error << std::endl;
+ }
/* If too much memory has been allocated, resize. */
error.conservativeResize(cursorError);
} catch SOT_RETHROW;
- sotDEBUG(35) << "error_final: "<< error << std::endl;
+ sotDEBUG(35) << "error_final: " << error << std::endl;
sotDEBUG(15) << "# Out }" << endl;
return error;
}
-dynamicgraph::Vector& Task::
-computeErrorTimeDerivative( dynamicgraph::Vector & res, int time)
-{
- res.resize( errorSOUT(time).size() );
+dynamicgraph::Vector &
+Task::computeErrorTimeDerivative(dynamicgraph::Vector &res, int time) {
+ res.resize(errorSOUT(time).size());
dynamicgraph::Vector::Index cursor = 0;
- for( std::list< FeatureAbstract* >::iterator iter = featureList.begin();
- iter!=featureList.end(); ++iter )
- {
- FeatureAbstract &feature = **iter;
+ for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
+ iter != featureList.end(); ++iter) {
+ FeatureAbstract &feature = **iter;
- const dynamicgraph::Vector& partialErrorDot = feature.getErrorDot()(time);
- const dynamicgraph::Vector::Index dim = partialErrorDot.size();
- res.segment (cursor, dim) = partialErrorDot;
- cursor += dim;
- }
+ const dynamicgraph::Vector &partialErrorDot = feature.getErrorDot()(time);
+ const dynamicgraph::Vector::Index dim = partialErrorDot.size();
+ res.segment(cursor, dim) = partialErrorDot;
+ cursor += dim;
+ }
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);
- errorRef.resize( errSingleBound.size() );
-
- for( unsigned int i=0;i<errorRef.size(); ++i )
- errorRef[i] = - errSingleBound(i)*gain;
-
- if( withDerivative )
- {
- const dynamicgraph::Vector & de = errorTimeDerivativeSOUT(time);
- for( unsigned int i=0;i<errorRef.size(); ++i )
- errorRef[i] = errorRef[i].getSingleBound() - de(i);
- }
+ const dynamicgraph::Vector &errSingleBound = errorSOUT(time);
+ const double &gain = controlGainSIN(time);
+ errorRef.resize(errSingleBound.size());
+
+ for (unsigned int i = 0; i < errorRef.size(); ++i)
+ errorRef[i] = -errSingleBound(i) * gain;
+
+ if (withDerivative) {
+ const dynamicgraph::Vector &de = errorTimeDerivativeSOUT(time);
+ for (unsigned int i = 0; i < errorRef.size(); ++i)
+ errorRef[i] = errorRef[i].getSingleBound() - de(i);
+ }
sotDEBUG(15) << "# Out }" << endl;
return errorRef;
}
-dynamicgraph::Matrix& Task::
-computeJacobian( dynamicgraph::Matrix& J,int time )
-{
+dynamicgraph::Matrix &Task::computeJacobian(dynamicgraph::Matrix &J, int time) {
sotDEBUG(15) << "# In {" << endl;
- if( featureList.empty())
- { throw( ExceptionTask(ExceptionTask::EMPTY_LIST,
- "Empty feature list") ) ; }
+ if (featureList.empty()) {
+ throw(ExceptionTask(ExceptionTask::EMPTY_LIST, "Empty feature list"));
+ }
try {
- dynamicgraph::Matrix::Index dimJ = J .rows();
+ dynamicgraph::Matrix::Index dimJ = J.rows();
dynamicgraph::Matrix::Index nbc = J.cols();
- if( 0==dimJ ){ dimJ = 1; J.resize(dimJ,nbc); }
+ if (0 == dimJ) {
+ dimJ = 1;
+ J.resize(dimJ, nbc);
+ }
dynamicgraph::Matrix::Index cursorJ = 0;
- //const Flags& selection = controlSelectionSIN(time);
+ // const Flags& selection = controlSelectionSIN(time);
/* For each cell of the list, recopy value of s, s_star and error. */
- for( std::list< FeatureAbstract* >::iterator iter = featureList.begin();
- iter!=featureList.end(); ++iter )
- {
- FeatureAbstract &feature = ** iter;
- sotDEBUG(25) << "Feature <" << feature.getName() <<">"<< endl;
-
- /* Get s, and store it in the s vector. */
- const dynamicgraph::Matrix& partialJacobian = feature.jacobianSOUT(time);
- const dynamicgraph::Matrix::Index nbr = partialJacobian.rows();
- sotDEBUG(25) << "Jp =" <<endl<< partialJacobian<<endl;
-
- if( 0==nbc ) { nbc = partialJacobian.cols(); J.resize(nbc,dimJ); }
- else if( partialJacobian.cols() != nbc )
- throw ExceptionTask(ExceptionTask::NON_ADEQUATE_FEATURES,
- "Features from the list don't have compatible-size jacobians.");
-
- while( cursorJ+nbr>=dimJ )
- { dimJ *= 2; J.conservativeResize(dimJ,nbc); }
- // TODO If controlSelectionSIN is really to be removed,
- // then the following loop is equivalent to:
- // J.middleRows (cursorJ, nbr) = partialJacobian;
- for( int kc=0;kc<nbc;++kc )
- {
- // if( selection(kc) )
- for( unsigned int k=0;k<nbr;++k )
- { J(cursorJ+k,kc) = partialJacobian(k,kc); }
- // else
- // for( unsigned int k=0;k<nbr;++k ) J(cursorJ+k,kc) = 0.;
- }
- cursorJ += nbr;
+ for (std::list<FeatureAbstract *>::iterator iter = featureList.begin();
+ iter != featureList.end(); ++iter) {
+ FeatureAbstract &feature = **iter;
+ sotDEBUG(25) << "Feature <" << feature.getName() << ">" << endl;
+
+ /* Get s, and store it in the s vector. */
+ const dynamicgraph::Matrix &partialJacobian = feature.jacobianSOUT(time);
+ const dynamicgraph::Matrix::Index nbr = partialJacobian.rows();
+ sotDEBUG(25) << "Jp =" << endl << partialJacobian << endl;
+
+ if (0 == nbc) {
+ nbc = partialJacobian.cols();
+ J.resize(nbc, dimJ);
+ } else if (partialJacobian.cols() != nbc)
+ throw ExceptionTask(
+ ExceptionTask::NON_ADEQUATE_FEATURES,
+ "Features from the list don't have compatible-size jacobians.");
+
+ while (cursorJ + nbr >= dimJ) {
+ dimJ *= 2;
+ J.conservativeResize(dimJ, nbc);
+ }
+ // TODO If controlSelectionSIN is really to be removed,
+ // then the following loop is equivalent to:
+ // J.middleRows (cursorJ, nbr) = partialJacobian;
+ for (int kc = 0; kc < nbc; ++kc) {
+ // if( selection(kc) )
+ for (unsigned int k = 0; k < nbr; ++k) {
+ J(cursorJ + k, kc) = partialJacobian(k, kc);
+ }
+ // else
+ // for( unsigned int k=0;k<nbr;++k ) J(cursorJ+k,kc) = 0.;
}
+ cursorJ += nbr;
+ }
/* If too much memory has been allocated, resize. */
- J .conservativeResize(cursorJ,nbc);
+ J.conservativeResize(cursorJ, nbc);
} catch SOT_RETHROW;
-
sotDEBUG(15) << "# Out }" << endl;
return J;
}
@@ -333,30 +311,23 @@ computeJacobian( dynamicgraph::Matrix& J,int time )
/* --- DISPLAY ------------------------------------------------------------ */
/* --- DISPLAY ------------------------------------------------------------ */
-void Task::
-display( std::ostream& os ) const
-{
+void Task::display(std::ostream &os) const {
os << "Task " << name << ": " << endl;
os << "--- LIST --- " << std::endl;
- for( std::list< FeatureAbstract* >::const_iterator iter = featureList.begin();
- iter!=featureList.end(); ++iter )
- {
- os << "-> " << (*iter)->getName() <<endl;
- }
-
+ for (std::list<FeatureAbstract *>::const_iterator iter = featureList.begin();
+ iter != featureList.end(); ++iter) {
+ os << "-> " << (*iter)->getName() << endl;
+ }
}
-
-std::ostream & Task::
-writeGraph(std::ostream &os) const
-{
- std::list< FeatureAbstract * >::const_iterator itFeatureAbstract;
+std::ostream &Task::writeGraph(std::ostream &os) const {
+ std::list<FeatureAbstract *>::const_iterator itFeatureAbstract;
itFeatureAbstract = featureList.begin();
- while(itFeatureAbstract!=featureList.end())
- {
- os << "\t\"" << (*itFeatureAbstract)->getName() << "\" -> \"" << getName() << "\"" << endl;
- itFeatureAbstract++;
- }
+ while (itFeatureAbstract != featureList.end()) {
+ os << "\t\"" << (*itFeatureAbstract)->getName() << "\" -> \"" << getName()
+ << "\"" << endl;
+ itFeatureAbstract++;
+ }
return os;
}
diff --git a/src/tools/binary-int-to-uint.cpp b/src/tools/binary-int-to-uint.cpp
index 8abf28d4..531fc0cc 100644
--- a/src/tools/binary-int-to-uint.cpp
+++ b/src/tools/binary-int-to-uint.cpp
@@ -8,10 +8,10 @@
*/
/* --- SOT --- */
-#include <sot/core/pool.hh>
#include <sot/core/binary-int-to-uint.hh>
-#include <sot/core/exception-feature.hh>
#include <sot/core/debug.hh>
+#include <sot/core/exception-feature.hh>
+#include <sot/core/pool.hh>
using namespace std;
#include <dynamic-graph/factory.h>
@@ -19,39 +19,40 @@ using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BinaryIntToUint,"BinaryIntToUint");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BinaryIntToUint, "BinaryIntToUint");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-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" )
-{
- signalRegistration( binaryIntSIN<<binaryUintSOUT );
+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") {
+ signalRegistration(binaryIntSIN << binaryUintSOUT);
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-unsigned& BinaryIntToUint::computeOutput( unsigned& res,int time )
-{
+unsigned &BinaryIntToUint::computeOutput(unsigned &res, int time) {
sotDEBUGIN(15);
int in = binaryIntSIN.access(time);
- if(in < 0){ res = 0; }
- else{ res = 1; }
+ if (in < 0) {
+ res = 0;
+ } else {
+ res = 1;
+ }
sotDEBUGOUT(15);
return res;
}
-void BinaryIntToUint::display( std::ostream& os ) const
-{
+void BinaryIntToUint::display(std::ostream &os) const {
os << "BinaryIntToUint <" << name << "> TODO..." << endl;
}
diff --git a/src/tools/clamp-workspace.cpp b/src/tools/clamp-workspace.cpp
index 346b9784..11a62111 100644
--- a/src/tools/clamp-workspace.cpp
+++ b/src/tools/clamp-workspace.cpp
@@ -18,43 +18,41 @@ using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ClampWorkspace,"ClampWorkspace");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ClampWorkspace, "ClampWorkspace");
-ClampWorkspace::
-ClampWorkspace( const string& fName )
- : Entity( fName )
- ,positionrefSIN( NULL,"ClampWorkspace("+name+")::input(double)::positionref")
- ,positionSIN( NULL,"ClampWorkspace("+name+")::input(double)::position")
+ClampWorkspace::ClampWorkspace(const string &fName)
+ : Entity(fName), positionrefSIN(NULL, "ClampWorkspace(" + name +
+ ")::input(double)::positionref"),
+ positionSIN(NULL, "ClampWorkspace(" + name + ")::input(double)::position")
- ,alphaSOUT( boost::bind(&ClampWorkspace::computeOutput,this, _1, _2),
- positionrefSIN<<positionSIN,
- "ClampWorkspace("+name+")::output(vector)::alpha" )
+ ,
+ alphaSOUT(boost::bind(&ClampWorkspace::computeOutput, this, _1, _2),
+ positionrefSIN << positionSIN,
+ "ClampWorkspace(" + name + ")::output(vector)::alpha")
- ,alphabarSOUT( boost::bind(&ClampWorkspace::computeOutputBar,this, _1, _2),
- positionrefSIN<<positionSIN,
- "ClampWorkspace("+name+")::output(vector)::alphabar" )
+ ,
+ alphabarSOUT(boost::bind(&ClampWorkspace::computeOutputBar, this, _1, _2),
+ positionrefSIN << positionSIN,
+ "ClampWorkspace(" + name + ")::output(vector)::alphabar")
- ,handrefSOUT( boost::bind(&ClampWorkspace::computeRef,this, _1, _2),
- positionrefSIN<<positionSIN,
- "ClampWorkspace("+name+")::output(vector)::ref" )
+ ,
+ handrefSOUT(boost::bind(&ClampWorkspace::computeRef, this, _1, _2),
+ positionrefSIN << positionSIN,
+ "ClampWorkspace(" + name + ")::output(vector)::ref")
- ,timeUpdate(0)
+ ,
+ timeUpdate(0)
- ,alpha(6,6)
- ,alphabar(6,6)
+ ,
+ alpha(6, 6), alphabar(6, 6)
- ,pd(3)
+ ,
+ 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();
@@ -63,91 +61,102 @@ ClampWorkspace( const string& fName )
bounds[1] = std::make_pair(-0.4, -0.25);
bounds[2] = std::make_pair(0.15, 0.55);
- signalRegistration( positionrefSIN<<positionSIN<<
- alphaSOUT<<alphabarSOUT<<handrefSOUT );
+ signalRegistration(positionrefSIN << positionSIN << alphaSOUT << alphabarSOUT
+ << handrefSOUT);
}
-void ClampWorkspace::update( int time )
-{
- if( time<=timeUpdate ){ return; }
+void ClampWorkspace::update(int time) {
+ if (time <= timeUpdate) {
+ return;
+ }
alpha.setZero();
alphabar.setIdentity();
- const MatrixHomogeneous& posref = positionrefSIN.access( time );
- const MatrixHomogeneous& pos = positionSIN.access ( time );
+ const MatrixHomogeneous &posref = positionrefSIN.access(time);
+ const MatrixHomogeneous &pos = positionSIN.access(time);
MatrixHomogeneous prefMw = posref.inverse(Eigen::Affine);
- prefMp = prefMw*pos;
+ prefMp = prefMw * pos;
Vector x(prefMp.translation());
- for(int i = 0; i < 3; ++i) {
+ for (int i = 0; i < 3; ++i) {
double check_min = std::max(x(i) - bounds[i].first, 0.);
double check_max = std::max(bounds[i].second - x(i), 0.);
double dm = std::min(check_min, check_max);
double Y = (dm - dm_min) / (dm_max - dm_min);
- if(Y < 0){ Y = 0; }
- if(Y > 1){ Y = 1; }
+ if (Y < 0) {
+ Y = 0;
+ }
+ if (Y > 1) {
+ Y = 1;
+ }
- switch(mode) {
+ switch (mode) {
case 0:
- alpha(i,i) = 0;
- alphabar(i,i) = 1;
+ alpha(i, i) = 0;
+ alphabar(i, i) = 1;
break;
case 1:
- alpha(i,i) = 1;
- alphabar(i,i) = 0;
+ 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);
+ alpha(i, i) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
+ alphabar(i, i) = 1 - alpha(i);
}
- if(i == 2) {
+ if (i == 2) {
Y = (dm - dm_min_yaw) / (dm_max_yaw - dm_min_yaw);
- if(Y < 0){ Y = 0; }
- if(Y > 1){ Y = 1; }
- if(mode == 2) {
- alpha(i+3,i+3) = 0.5 * ( 1 + tanh(1/Y - 1/(1-Y)) );
- alphabar(i+3,i+3) = 1 - alpha(i+3);
+ if (Y < 0) {
+ Y = 0;
+ }
+ if (Y > 1) {
+ Y = 1;
+ }
+ if (mode == 2) {
+ alpha(i + 3, i + 3) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
+ alphabar(i + 3, i + 3) = 1 - alpha(i + 3);
}
}
}
- if(frame == FRAME_POINT) {
+ if (frame == FRAME_POINT) {
MatrixHomogeneous prefMp_tmp = prefMp;
MatrixHomogeneous pMpref = prefMp.inverse(Eigen::Affine);
- for( int i = 0;i<3;++i ) {
- pMpref(i,3) = 0;
- prefMp_tmp(i,3) = 0;
+ for (int i = 0; i < 3; ++i) {
+ pMpref(i, 3) = 0;
+ prefMp_tmp(i, 3) = 0;
}
- MatrixTwist pTpref; buildFrom(pMpref, pTpref );
- MatrixTwist prefTp; buildFrom(prefMp_tmp, prefTp );
+ MatrixTwist pTpref;
+ buildFrom(pMpref, pTpref);
+ MatrixTwist prefTp;
+ buildFrom(prefMp_tmp, prefTp);
- Matrix tmp; tmp = alpha*prefTp;
- alpha = pTpref*tmp;
+ Matrix tmp;
+ tmp = alpha * prefTp;
+ alpha = pTpref * tmp;
- tmp = alphabar*prefTp;
- alphabar = pTpref*tmp;
+ tmp = alphabar * prefTp;
+ alphabar = pTpref * tmp;
}
- for(int i = 0; i < 3; ++i) {
+ for (int i = 0; i < 3; ++i) {
pd(i) = 0.5 * (bounds[i].first + bounds[i].second);
}
VectorRollPitchYaw rpy;
rpy(0) = 0;
rpy(1) = -3.14159256 / 2;
- rpy(2) = theta_min +
- (theta_max - theta_min) *
- (x(1) - bounds[1].first)/(bounds[1].second - bounds[1].first);
+ rpy(2) = theta_min + (theta_max - theta_min) * (x(1) - bounds[1].first) /
+ (bounds[1].second - bounds[1].first);
- Eigen::Affine3d _Rd(Eigen::AngleAxisd(rpy(2),Eigen::Vector3d::UnitZ())*
- Eigen::AngleAxisd(rpy(1),Eigen::Vector3d::UnitY())*
- Eigen::AngleAxisd(rpy(0),Eigen::Vector3d::UnitX()));
+ Eigen::Affine3d _Rd(Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()));
Rd = _Rd.linear();
Eigen::Affine3d _tmpaffine;
@@ -157,36 +166,30 @@ void ClampWorkspace::update( int time )
timeUpdate = time;
}
-Matrix&
-ClampWorkspace::computeOutput( Matrix& res,int time )
-{
+Matrix &ClampWorkspace::computeOutput(Matrix &res, int time) {
update(time);
res = alpha;
return res;
}
-Matrix&
-ClampWorkspace::computeOutputBar( Matrix& res,int time )
-{
+Matrix &ClampWorkspace::computeOutputBar(Matrix &res, int time) {
update(time);
res = alphabar;
return res;
}
-MatrixHomogeneous&
-ClampWorkspace::computeRef( MatrixHomogeneous& res,int time )
-{
+MatrixHomogeneous &ClampWorkspace::computeRef(MatrixHomogeneous &res,
+ int time) {
update(time);
res = handref;
return res;
}
-void ClampWorkspace::display( std:: ostream& os ) const
-{
+void ClampWorkspace::display(std::ostream &os) const {
os << "ClampWorkspace<" << name << ">" << endl << endl;
os << "alpha: " << alpha << endl;
os << "pos in ws: " << prefMp << endl;
os << "bounds: " << bounds[0].first << " " << bounds[0].second << " "
- << bounds[1].first << " " << bounds[1].second << " "
- << bounds[2].first << " " << bounds[2].second << endl;
+ << bounds[1].first << " " << bounds[1].second << " " << bounds[2].first
+ << " " << bounds[2].second << endl;
}
diff --git a/src/tools/com-freezer.cpp b/src/tools/com-freezer.cpp
index 6da83dcf..175ebc06 100644
--- a/src/tools/com-freezer.cpp
+++ b/src/tools/com-freezer.cpp
@@ -7,61 +7,56 @@
*
*/
+#include <dynamic-graph/factory.h>
#include <sot/core/com-freezer.hh>
#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(CoMFreezer, "CoMFreezer");
-CoMFreezer::CoMFreezer(const std::string & name)
- : Entity(name)
- , m_lastCoM(3)
- , m_previousPGInProcess(false)
- , m_lastStopTime(-1)
-
- , CoMRefSIN(NULL, "CoMFreezer("+name+")::input(vector)::CoMRef")
- , PGInProcessSIN(NULL, "CoMFreezer("+name+")::input(bool)::PGInProcess")
- , freezedCoMSOUT(boost::bind(&CoMFreezer::computeFreezedCoM, this, _1, _2),
- CoMRefSIN << PGInProcessSIN,
- "CoMFreezer("+name+")::output(vector)::freezedCoM")
-{
+CoMFreezer::CoMFreezer(const std::string &name)
+ : Entity(name), m_lastCoM(3), m_previousPGInProcess(false),
+ m_lastStopTime(-1)
+
+ ,
+ CoMRefSIN(NULL, "CoMFreezer(" + name + ")::input(vector)::CoMRef"),
+ PGInProcessSIN(NULL,
+ "CoMFreezer(" + name + ")::input(bool)::PGInProcess"),
+ freezedCoMSOUT(boost::bind(&CoMFreezer::computeFreezedCoM, this, _1, _2),
+ CoMRefSIN << PGInProcessSIN,
+ "CoMFreezer(" + name + ")::output(vector)::freezedCoM") {
sotDEBUGIN(5);
- signalRegistration( CoMRefSIN << PGInProcessSIN << freezedCoMSOUT );
+ signalRegistration(CoMRefSIN << PGInProcessSIN << freezedCoMSOUT);
sotDEBUGOUT(5);
}
-CoMFreezer::~CoMFreezer(void)
-{
+CoMFreezer::~CoMFreezer(void) {
sotDEBUGIN(5);
sotDEBUGOUT(5);
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);
- if(PGInProcess) /* CoM unfreezed */
+ if (PGInProcess) /* CoM unfreezed */
{
m_lastCoM = CoMRefSIN(time);
m_previousPGInProcess = (PGInProcess == 0);
- }
- else
- {
- if(m_previousPGInProcess) /* pg.inprocess switch from 1 to 0 */
+ } else {
+ if (m_previousPGInProcess) /* pg.inprocess switch from 1 to 0 */
{
m_lastStopTime = time;
m_lastCoM = CoMRefSIN(time);
m_previousPGInProcess = (PGInProcess == 0);
- }
- else if(time < m_lastStopTime + 200) /* keep updating for 1s */
+ } else if (time < m_lastStopTime + 200) /* keep updating for 1s */
{
m_lastCoM = CoMRefSIN(time);
}
@@ -71,19 +66,16 @@ dynamicgraph::Vector & CoMFreezer::computeFreezedCoM(dynamicgraph::Vector & free
sotDEBUGOUT(15);
- if(m_lastStopTime < 0)
- {
+ if (m_lastStopTime < 0) {
m_lastCoM = CoMRefSIN(time);
m_lastStopTime = time;
freezedCoM = m_lastCoM;
return freezedCoM;
}
-
return m_lastCoM;
}
-void CoMFreezer::display(std::ostream & os) const
-{
+void CoMFreezer::display(std::ostream &os) const {
os << "CoMFreezer " << getName() << "." << std::endl;
}
diff --git a/src/tools/device.cpp b/src/tools/device.cpp
index d056425f..d7b6d428 100644
--- a/src/tools/device.cpp
+++ b/src/tools/device.cpp
@@ -13,16 +13,16 @@
/* SOT */
#define ENABLE_RT_LOG
-#include <iostream>
#include "sot/core/device.hh"
+#include <iostream>
#include <sot/core/debug.hh>
using namespace std;
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/real-time-logger.h>
-#include <dynamic-graph/all-commands.h>
#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 <pinocchio/multibody/liegroup/special-euclidean.hpp>
@@ -35,551 +35,466 @@ const std::string Device::CLASS_NAME = "Device";
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void Device::
-integrateRollPitchYaw
-(Vector& state,
- const Vector& control,
- double dt)
-{
+void Device::integrateRollPitchYaw(Vector &state, const Vector &control,
+ double dt) {
using Eigen::AngleAxisd;
- using Eigen::Vector3d;
using Eigen::QuaternionMapd;
+ using Eigen::Vector3d;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
Eigen::Matrix<double, 7, 1> qin, qout;
qin.head<3>() = state.head<3>();
- QuaternionMapd quat (qin.tail<4>().data());
- quat = AngleAxisd(state(5), Vector3d::UnitZ())
- * AngleAxisd(state(4), Vector3d::UnitY())
- * AngleAxisd(state(3), Vector3d::UnitX());
+ QuaternionMapd quat(qin.tail<4>().data());
+ quat = AngleAxisd(state(5), Vector3d::UnitZ()) *
+ AngleAxisd(state(4), Vector3d::UnitY()) *
+ AngleAxisd(state(3), Vector3d::UnitX());
- SE3().integrate (qin, control.head<6>()*dt, qout);
+ SE3().integrate(qin, control.head<6>() * dt, qout);
// Update freeflyer pose
ffPose_.translation() = qout.head<3>();
state.head<3>() = qout.head<3>();
- ffPose_.linear() =
- QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
- state.segment<3>(3) = ffPose_.linear().eulerAngles(2,1,0).reverse();
+ ffPose_.linear() = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
+ state.segment<3>(3) = ffPose_.linear().eulerAngles(2, 1, 0).reverse();
}
-const MatrixHomogeneous&
-Device::freeFlyerPose() const
-{
- return ffPose_;
-}
+const MatrixHomogeneous &Device::freeFlyerPose() const { return ffPose_; }
-Device::
-~Device( )
-{
- for( unsigned int i=0; i<4; ++i ) {
+Device::~Device() {
+ for (unsigned int i = 0; i < 4; ++i) {
delete forcesSOUT[i];
}
}
-Device::
-Device( const std::string& n )
- :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")
- ,zmpSIN(NULL,"Device("+n+")::input(vector3)::zmp")
- ,stateSOUT( "Device("+n+")::output(vector)::state" )
- //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
- ,velocitySOUT( "Device("+n+")::output(vector)::velocity" )
- ,attitudeSOUT( "Device("+n+")::output(matrixRot)::attitude" )
- ,motorcontrolSOUT ( "Device("+n+")::output(vector)::motorcontrol" )
- ,previousControlSOUT( "Device("+n+")::output(vector)::previousControl" )
- ,ZMPPreviousControllerSOUT
- ( "Device("+n+
- ")::output(vector)::zmppreviouscontroller" )
- ,robotState_ ("Device("+n+")::output(vector)::robotState")
- ,robotVelocity_ ("Device("+n+")::output(vector)::robotVelocity")
- ,pseudoTorqueSOUT("Device("+n+")::output(vector)::ptorque" )
-
- ,ffPose_()
- ,forceZero6 (6)
-{
- forceZero6.fill (0);
+Device::Device(const std::string &n)
+ : 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"),
+ zmpSIN(NULL, "Device(" + n + ")::input(vector3)::zmp"),
+ stateSOUT("Device(" + n + ")::output(vector)::state")
+ //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
+ ,
+ velocitySOUT("Device(" + n + ")::output(vector)::velocity"),
+ attitudeSOUT("Device(" + n + ")::output(matrixRot)::attitude"),
+ motorcontrolSOUT("Device(" + n + ")::output(vector)::motorcontrol"),
+ previousControlSOUT("Device(" + n + ")::output(vector)::previousControl"),
+ ZMPPreviousControllerSOUT("Device(" + n +
+ ")::output(vector)::zmppreviouscontroller"),
+ robotState_("Device(" + n + ")::output(vector)::robotState"),
+ robotVelocity_("Device(" + n + ")::output(vector)::robotVelocity"),
+ pseudoTorqueSOUT("Device(" + n + ")::output(vector)::ptorque")
+
+ ,
+ ffPose_(), forceZero6(6) {
+ forceZero6.fill(0);
/* --- SIGNALS --- */
- for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
+ for (int i = 0; i < 4; ++i) {
+ withForceSignals[i] = false;
+ }
forcesSOUT[0] =
- new Signal<Vector, int>("Device("+n+")::output(vector6)::forceRLEG");
+ new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRLEG");
forcesSOUT[1] =
- new Signal<Vector, int>("Device("+n+")::output(vector6)::forceLLEG");
+ new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLLEG");
forcesSOUT[2] =
- new Signal<Vector, int>("Device("+n+")::output(vector6)::forceRARM");
+ new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRARM");
forcesSOUT[3] =
- new Signal<Vector, int>("Device("+n+")::output(vector6)::forceLARM");
+ new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLARM");
- signalRegistration( controlSIN<<stateSOUT<<robotState_<<robotVelocity_
- <<velocitySOUT<<attitudeSOUT
- <<attitudeSIN<<zmpSIN <<*forcesSOUT[0]<<*forcesSOUT[1]
- <<*forcesSOUT[2]<<*forcesSOUT[3] <<previousControlSOUT
- <<pseudoTorqueSOUT << motorcontrolSOUT
- << ZMPPreviousControllerSOUT );
- state_.fill(.0); stateSOUT.setConstant( state_ );
+ signalRegistration(
+ controlSIN << stateSOUT << robotState_ << robotVelocity_ << velocitySOUT
+ << attitudeSOUT << attitudeSIN << zmpSIN << *forcesSOUT[0]
+ << *forcesSOUT[1] << *forcesSOUT[2] << *forcesSOUT[3]
+ << previousControlSOUT << pseudoTorqueSOUT << motorcontrolSOUT
+ << ZMPPreviousControllerSOUT);
+ state_.fill(.0);
+ stateSOUT.setConstant(state_);
- velocity_.resize(state_.size()); velocity_.setZero();
- velocitySOUT.setConstant( velocity_ );
+ velocity_.resize(state_.size());
+ velocity_.setZero();
+ velocitySOUT.setConstant(velocity_);
/* --- Commands --- */
{
std::string docstring;
/* Command setStateSize. */
- 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";
- addCommand("set",
- new command::Setter<Device, Vector>
- (*this, &Device::setState, docstring));
-
- docstring =
- "\n"
- " Set velocity vector value\n"
- "\n";
- addCommand("setVelocity",
- new command::Setter<Device, Vector>
- (*this, &Device::setVelocity, docstring));
-
- void(Device::*setRootPtr)(const Matrix&) = &Device::setRoot;
- docstring
- = command::docCommandVoid1("Set the root position.",
- "matrix homogeneous");
+ 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";
+ addCommand("set", new command::Setter<Device, Vector>(
+ *this, &Device::setState, docstring));
+
+ docstring = "\n"
+ " Set velocity vector value\n"
+ "\n";
+ addCommand("setVelocity", new command::Setter<Device, Vector>(
+ *this, &Device::setVelocity, docstring));
+
+ void (Device::*setRootPtr)(const Matrix &) = &Device::setRoot;
+ docstring = command::docCommandVoid1("Set the root position.",
+ "matrix homogeneous");
addCommand("setRoot",
- command::makeCommandVoid1(*this,setRootPtr,
- docstring));
+ 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";
-
- addCommand
- ("setSecondOrderIntegration",
- command::makeCommandVoid0
- (*this,&Device::setSecondOrderIntegration,
- docstring));
+ 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";
- addCommand
- ("display",
- command::makeCommandVoid0
- (*this,&Device::cmdDisplay,docstring));
+ 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));
+ 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));
+ new command::Setter<Device, bool>(*this, &Device::setSanityCheck,
+ docstring));
addCommand("setPositionBounds",
- command::makeCommandVoid2
- (*this,&Device::setPositionBounds,
- command::docCommandVoid2
- ("Set robot position bounds",
- "vector: lower bounds",
- "vector: upper bounds")));
+ command::makeCommandVoid2(
+ *this, &Device::setPositionBounds,
+ command::docCommandVoid2("Set robot position bounds",
+ "vector: lower bounds",
+ "vector: upper bounds")));
addCommand("setVelocityBounds",
- command::makeCommandVoid2
- (*this,&Device::setVelocityBounds,
- command::docCommandVoid2
- ("Set robot velocity bounds",
- "vector: lower bounds",
- "vector: upper bounds")));
+ command::makeCommandVoid2(
+ *this, &Device::setVelocityBounds,
+ command::docCommandVoid2("Set robot velocity bounds",
+ "vector: lower bounds",
+ "vector: upper bounds")));
addCommand("setTorqueBounds",
- command::makeCommandVoid2
- (*this,&Device::setTorqueBounds,
- command::docCommandVoid2
- ("Set robot torque bounds",
- "vector: lower bounds",
- "vector: upper bounds")));
+ command::makeCommandVoid2(
+ *this, &Device::setTorqueBounds,
+ command::docCommandVoid2("Set robot torque bounds",
+ "vector: lower bounds",
+ "vector: upper bounds")));
addCommand("getTimeStep",
- command::makeDirectGetter
- (*this, &this->timestep_,
- command::docDirectGetter
- ("Time step", "double")));
+ command::makeDirectGetter(
+ *this, &this->timestep_,
+ command::docDirectGetter("Time step", "double")));
// Handle commands and signals called in a synchronous way.
periodicCallBefore_.addSpecificCommands(*this, commandMap, "before.");
periodicCallAfter_.addSpecificCommands(*this, commandMap, "after.");
-
}
}
-void Device::
-setStateSize( const unsigned int& size )
-{
- state_.resize(size); state_.fill( .0 );
- stateSOUT .setConstant( state_ );
- previousControlSOUT.setConstant( state_ );
- pseudoTorqueSOUT.setConstant( state_ );
- motorcontrolSOUT .setConstant( state_ );
+void Device::setStateSize(const unsigned int &size) {
+ state_.resize(size);
+ state_.fill(.0);
+ stateSOUT.setConstant(state_);
+ previousControlSOUT.setConstant(state_);
+ pseudoTorqueSOUT.setConstant(state_);
+ motorcontrolSOUT.setConstant(state_);
Device::setVelocitySize(size);
- Vector zmp(3); zmp.fill( .0 );
- ZMPPreviousControllerSOUT .setConstant( zmp );
+ Vector zmp(3);
+ zmp.fill(.0);
+ ZMPPreviousControllerSOUT.setConstant(zmp);
}
-void Device::
-setVelocitySize( const unsigned int& size )
-{
+void Device::setVelocitySize(const unsigned int &size) {
velocity_.resize(size);
velocity_.fill(.0);
- velocitySOUT.setConstant( velocity_ );
+ velocitySOUT.setConstant(velocity_);
}
-void Device::
-setState( const Vector& st )
-{
+void Device::setState(const Vector &st) {
if (sanityCheck_) {
- const Vector::Index& s = st.size();
+ const Vector::Index &s = st.size();
switch (controlInputType_) {
- case CONTROL_INPUT_TWO_INTEGRATION:
- dgRTLOG()
+ 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");
- 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");
- case CONTROL_INPUT_NO_INTEGRATION:
- break;
- default:
- throw std::invalid_argument ("Invalid control mode type.");
+ 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");
+ 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");
+ case CONTROL_INPUT_NO_INTEGRATION:
+ break;
+ default:
+ throw std::invalid_argument("Invalid control mode type.");
}
}
state_ = st;
- stateSOUT .setConstant( state_ );
- motorcontrolSOUT .setConstant( state_ );
+ stateSOUT.setConstant(state_);
+ motorcontrolSOUT.setConstant(state_);
}
-void Device::
-setVelocity( const Vector& vel )
-{
+void Device::setVelocity(const Vector &vel) {
velocity_ = vel;
- velocitySOUT .setConstant( velocity_ );
+ velocitySOUT.setConstant(velocity_);
}
-void Device::
-setRoot( const Matrix & root )
-{
+void Device::setRoot(const Matrix &root) {
Eigen::Matrix4d _matrix4d(root);
MatrixHomogeneous _root(_matrix4d);
- setRoot( _root );
+ setRoot(_root);
}
-void Device::
-setRoot( const MatrixHomogeneous & worldMwaist )
-{
- VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2,1,0)).reverse();
+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);
+ for (unsigned int i = 0; i < 3; ++i)
+ q(i + 3) = r(i);
}
-void Device::
-setSecondOrderIntegration()
-{
+void Device::setSecondOrderIntegration() {
controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
velocity_.resize(state_.size());
velocity_.setZero();
- velocitySOUT.setConstant( velocity_ );
+ velocitySOUT.setConstant(velocity_);
}
-void Device::
-setNoIntegration()
-{
+void Device::setNoIntegration() {
controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
velocity_.resize(state_.size());
velocity_.setZero();
- velocitySOUT.setConstant( velocity_ );
+ velocitySOUT.setConstant(velocity_);
}
-void Device::
-setControlInputType(const std::string& cit)
-{
- for(int i=0; i<CONTROL_INPUT_SIZE; i++)
- if(cit==ControlInput_s[i])
- {
+void Device::setControlInputType(const std::string &cit) {
+ for (int i = 0; i < CONTROL_INPUT_SIZE; i++)
+ if (cit == ControlInput_s[i]) {
controlInputType_ = (ControlInput)i;
- sotDEBUG(25)<<"Control input type: "<<ControlInput_s[i]<<endl;
+ sotDEBUG(25) << "Control input type: " << ControlInput_s[i] << endl;
return;
}
- sotDEBUG(25)<<"Unrecognized control input type: "<<cit<<endl;
+ sotDEBUG(25) << "Unrecognized control input type: " << cit << endl;
}
-void Device::
-setSanityCheck(const bool & enableCheck)
-{
+void Device::setSanityCheck(const bool &enableCheck) {
if (enableCheck) {
- const Vector::Index& s = state_.size();
+ const Vector::Index &s = state_.size();
switch (controlInputType_) {
- case CONTROL_INPUT_TWO_INTEGRATION:
- dgRTLOG()
+ 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");
- 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");
- 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.");
+ 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");
+ 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");
+ 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.");
}
}
sanityCheck_ = enableCheck;
}
-void Device::
-setPositionBounds(const Vector& lower, const Vector& upper)
-{
+void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
std::ostringstream oss;
if (lower.size() != state_.size()) {
oss << "Lower bound size should be " << state_.size();
- throw std::invalid_argument (oss.str());
+ throw std::invalid_argument(oss.str());
}
if (upper.size() != state_.size()) {
oss << "Upper bound size should be " << state_.size();
- throw std::invalid_argument (oss.str());
+ throw std::invalid_argument(oss.str());
}
lowerPosition_ = lower;
upperPosition_ = upper;
}
-void Device::
-setVelocityBounds(const Vector& lower, const Vector& upper)
-{
+void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
std::ostringstream oss;
if (lower.size() != velocity_.size()) {
oss << "Lower bound size should be " << velocity_.size();
- throw std::invalid_argument (oss.str());
+ throw std::invalid_argument(oss.str());
}
if (upper.size() != velocity_.size()) {
oss << "Upper bound size should be " << velocity_.size();
- throw std::invalid_argument (oss.str());
+ throw std::invalid_argument(oss.str());
}
lowerVelocity_ = lower;
upperVelocity_ = upper;
}
-void Device::
-setTorqueBounds (const Vector& lower, const Vector& upper)
-{
+void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
// TODO I think the torque bounds size are state_.size()-6...
std::ostringstream oss;
if (lower.size() != state_.size()) {
oss << "Lower bound size should be " << state_.size();
- throw std::invalid_argument (oss.str());
+ throw std::invalid_argument(oss.str());
}
if (upper.size() != state_.size()) {
oss << "Lower bound size should be " << state_.size();
- throw std::invalid_argument (oss.str());
+ throw std::invalid_argument(oss.str());
}
lowerTorque_ = lower;
upperTorque_ = upper;
}
-void Device::
-increment( const double & dt )
-{
+void Device::increment(const double &dt) {
int time = stateSOUT.getTime();
sotDEBUG(25) << "Time : " << time << std::endl;
// Run Synchronous commands and evaluate signals outside the main
// connected component of the graph.
- try
- {
- periodicCallBefore_.run(time+1);
+ try {
+ periodicCallBefore_.run(time + 1);
+ } catch (std::exception &e) {
+ dgRTLOG() << "exception caught while running periodical commands (before): "
+ << e.what() << std::endl;
+ } catch (const char *str) {
+ dgRTLOG() << "exception caught while running periodical commands (before): "
+ << str << std::endl;
+ } catch (...) {
+ dgRTLOG() << "unknown exception caught while"
+ << " running periodical commands (before)" << std::endl;
}
- catch (std::exception& e)
- {
- dgRTLOG()
- << "exception caught while running periodical commands (before): "
- << e.what () << std::endl;
- }
- catch (const char* str)
- {
- dgRTLOG()
- << "exception caught while running periodical commands (before): "
- << str << std::endl;
- }
- catch (...)
- {
- dgRTLOG()
- << "unknown exception caught while"
- << " running periodical commands (before)" << std::endl;
- }
-
/* Force the recomputation of the control. */
- controlSIN( time );
- sotDEBUG(25) << "u" <<time<<" = " << controlSIN.accessCopy() << endl;
+ controlSIN(time);
+ sotDEBUG(25) << "u" << time << " = " << controlSIN.accessCopy() << endl;
/* Integration of numerical values. This function is virtual. */
- integrate( dt );
+ integrate(dt);
sotDEBUG(25) << "q" << time << " = " << state_ << endl;
/* Position the signals corresponding to sensors. */
- stateSOUT .setConstant( state_ ); stateSOUT.setTime( time+1 );
- //computation of the velocity signal
- if( controlInputType_==CONTROL_INPUT_TWO_INTEGRATION )
- {
- velocitySOUT.setConstant( velocity_ );
- velocitySOUT.setTime( time+1 );
+ stateSOUT.setConstant(state_);
+ stateSOUT.setTime(time + 1);
+ // computation of the velocity signal
+ if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
+ velocitySOUT.setConstant(velocity_);
+ velocitySOUT.setTime(time + 1);
+ } else if (controlInputType_ == CONTROL_INPUT_ONE_INTEGRATION) {
+ velocitySOUT.setConstant(controlSIN.accessCopy());
+ velocitySOUT.setTime(time + 1);
}
- else if (controlInputType_==CONTROL_INPUT_ONE_INTEGRATION)
- {
- velocitySOUT.setConstant( controlSIN.accessCopy() );
- velocitySOUT.setTime( time+1 );
- }
- for( int i=0;i<4;++i ){
- if( !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
+ for (int i = 0; i < 4; ++i) {
+ if (!withForceSignals[i])
+ forcesSOUT[i]->setConstant(forceZero6);
}
- Vector zmp(3); zmp.fill( .0 );
- ZMPPreviousControllerSOUT .setConstant( zmp );
+ Vector zmp(3);
+ zmp.fill(.0);
+ ZMPPreviousControllerSOUT.setConstant(zmp);
// Run Synchronous commands and evaluate signals outside the main
// connected component of the graph.
- try
- {
- periodicCallAfter_.run(time+1);
+ try {
+ periodicCallAfter_.run(time + 1);
+ } catch (std::exception &e) {
+ dgRTLOG() << "exception caught while running periodical commands (after): "
+ << e.what() << std::endl;
+ } catch (const char *str) {
+ dgRTLOG() << "exception caught while running periodical commands (after): "
+ << str << std::endl;
+ } catch (...) {
+ dgRTLOG() << "unknown exception caught while"
+ << " running periodical commands (after)" << std::endl;
}
- catch (std::exception& e)
- {
- dgRTLOG()
- << "exception caught while running periodical commands (after): "
- << e.what () << std::endl;
- }
- catch (const char* str)
- {
- dgRTLOG()
- << "exception caught while running periodical commands (after): "
- << str << std::endl;
- }
- catch (...)
- {
- dgRTLOG()
- << "unknown exception caught while"
- << " running periodical commands (after)" << std::endl;
- }
-
// Others signals.
- motorcontrolSOUT .setConstant( state_ );
+ motorcontrolSOUT.setConstant(state_);
}
// Return true if it saturates.
-inline bool
-saturateBounds (double& val, const double& lower, const double& upper)
-{
- assert (lower <= upper);
- if (val < lower) { val = lower; return true; }
- if (upper < val) { val = upper; return true; }
+inline bool saturateBounds(double &val, const double &lower,
+ const double &upper) {
+ assert(lower <= upper);
+ if (val < lower) {
+ val = lower;
+ return true;
+ }
+ if (upper < val) {
+ val = upper;
+ return true;
+ }
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 )
-{
- const Vector & controlIN = controlSIN.accessCopy();
+void Device::integrate(const double &dt) {
+ const Vector &controlIN = controlSIN.accessCopy();
- if (sanityCheck_ && controlIN.hasNaN())
- {
- dgRTLOG () << "Device::integrate: Control has NaN values: " << '\n'
- << controlIN.transpose() << '\n';
+ if (sanityCheck_ && controlIN.hasNaN()) {
+ dgRTLOG() << "Device::integrate: Control has NaN values: " << '\n'
+ << controlIN.transpose() << '\n';
return;
}
- if (controlInputType_==CONTROL_INPUT_NO_INTEGRATION)
- {
+ if (controlInputType_ == CONTROL_INPUT_NO_INTEGRATION) {
state_.tail(controlIN.size()) = controlIN;
return;
}
- if( vel_control_.size() == 0 )
+ 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
// freedom as a translation and roll pitch yaw.
- if (controlInputType_==CONTROL_INPUT_TWO_INTEGRATION)
- {
+ if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
// TODO check acceleration
// Position increment
- vel_control_ = velocity_.tail(controlIN.size()) + (0.5*dt)*controlIN;
+ vel_control_ = velocity_.tail(controlIN.size()) + (0.5 * dt) * controlIN;
// Velocity integration.
- velocity_.tail(controlIN.size()) += controlIN*dt;
- }
- else
- {
+ velocity_.tail(controlIN.size()) += controlIN * dt;
+ } else {
vel_control_ = controlIN;
}
@@ -592,9 +507,8 @@ void Device::integrate( const double & dt )
// Freeflyer integration
integrateRollPitchYaw(state_, vel_control_, dt);
// Joints integration
- state_.tail(state_.size()-6) += vel_control_.tail(state_.size()-6) * dt;
- }
- else{
+ state_.tail(state_.size() - 6) += vel_control_.tail(state_.size() - 6) * dt;
+ } else {
// Position integration
state_.tail(controlIN.size()) += vel_control_ * dt;
}
@@ -607,16 +521,14 @@ void Device::integrate( const double & dt )
/* --- DISPLAY ------------------------------------------------------------ */
-void Device::display ( std::ostream& os ) const
-{
- os << name <<": "<<state_<<endl
- << "sanityCheck: " << sanityCheck_<< endl
+void Device::display(std::ostream &os) const {
+ os << name << ": " << state_ << endl
+ << "sanityCheck: " << sanityCheck_ << endl
<< "controlInputType:" << controlInputType_ << endl;
}
-void Device::cmdDisplay ( )
-{
- std::cout << name <<": "<<state_<<endl
- << "sanityCheck: " << sanityCheck_<< endl
- << "controlInputType:" << controlInputType_ << endl;
+void Device::cmdDisplay() {
+ std::cout << name << ": " << state_ << endl
+ << "sanityCheck: " << sanityCheck_ << endl
+ << "controlInputType:" << controlInputType_ << endl;
}
diff --git a/src/tools/event.cpp b/src/tools/event.cpp
index 314ea3b2..c62aa3d1 100644
--- a/src/tools/event.cpp
+++ b/src/tools/event.cpp
@@ -6,24 +6,23 @@
#include <dynamic-graph/factory.h>
namespace dynamicgraph {
- namespace sot {
+namespace sot {
- bool& Event::check (bool& ret, const int& time)
- {
- const bool& val = conditionSIN (time);
- ret = (val != lastVal_);
- bool trigger = onlyUp_ ? (!lastVal_ && val) : ret;
- if (ret) {
- lastVal_ = val;
- if (trigger) {
- for (Triggers_t::const_iterator _s = triggers.begin();
- _s != triggers.end(); ++_s)
- (*_s)->recompute (time);
- }
- }
- return ret;
+bool &Event::check(bool &ret, const int &time) {
+ const bool &val = conditionSIN(time);
+ ret = (val != lastVal_);
+ bool trigger = onlyUp_ ? (!lastVal_ && val) : ret;
+ if (ret) {
+ lastVal_ = val;
+ if (trigger) {
+ for (Triggers_t::const_iterator _s = triggers.begin();
+ _s != triggers.end(); ++_s)
+ (*_s)->recompute(time);
}
+ }
+ return ret;
+}
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (Event, "Event");
- } // namespace sot
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Event, "Event");
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/tools/exp-moving-avg.cpp b/src/tools/exp-moving-avg.cpp
index cad2d0f9..47863c81 100644
--- a/src/tools/exp-moving-avg.cpp
+++ b/src/tools/exp-moving-avg.cpp
@@ -11,8 +11,8 @@
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
-#include <sot/core/factory.hh>
#include <sot/core/exp-moving-avg.hh>
+#include <sot/core/factory.hh>
namespace dg = ::dynamicgraph;
@@ -21,60 +21,50 @@ namespace dg = ::dynamicgraph;
/* ---------------------------------------------------------------------------*/
namespace dynamicgraph {
- namespace sot {
-
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ExpMovingAvg,"ExpMovingAvg");
+namespace sot {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ExpMovingAvg, "ExpMovingAvg");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-ExpMovingAvg::
-ExpMovingAvg( const std::string& n )
- :Entity(n)
- ,updateSIN(NULL, "ExpMovingAvg(" + n + ")::input(vector)::update")
- ,refresherSINTERN( "ExpMovingAvg("+n+")::intern(dummy)::refresher" )
- ,averageSOUT(
- boost::bind(&ExpMovingAvg::update,this,_1,_2),
- updateSIN << refresherSINTERN, "ExpMovingAvg(" + n + ")::output(vector)::average")
- ,alpha(0.)
- ,init(false)
-{
+ExpMovingAvg::ExpMovingAvg(const std::string &n)
+ : Entity(n),
+ updateSIN(NULL, "ExpMovingAvg(" + n + ")::input(vector)::update"),
+ refresherSINTERN("ExpMovingAvg(" + n + ")::intern(dummy)::refresher"),
+ averageSOUT(boost::bind(&ExpMovingAvg::update, this, _1, _2),
+ updateSIN << refresherSINTERN,
+ "ExpMovingAvg(" + n + ")::output(vector)::average"),
+ alpha(0.), init(false) {
// Register signals into the entity.
signalRegistration(updateSIN << averageSOUT);
- refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
+ 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));
+ new ::dynamicgraph::command::Setter<ExpMovingAvg, double>(
+ *this, &ExpMovingAvg::setAlpha, docstring));
}
-ExpMovingAvg::~ExpMovingAvg()
-{
-}
+ExpMovingAvg::~ExpMovingAvg() {}
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
-void ExpMovingAvg::setAlpha(const double& alpha_) {
+void ExpMovingAvg::setAlpha(const double &alpha_) {
assert(alpha <= 1. && alpha >= 0.);
alpha = alpha_;
}
-dynamicgraph::Vector& ExpMovingAvg::update(dynamicgraph::Vector& res,
- const int& inTime)
-{
- const dynamicgraph::Vector& update = updateSIN(inTime);
+dynamicgraph::Vector &ExpMovingAvg::update(dynamicgraph::Vector &res,
+ const int &inTime) {
+ const dynamicgraph::Vector &update = updateSIN(inTime);
if (init == false) {
init = true;
@@ -87,7 +77,5 @@ dynamicgraph::Vector& ExpMovingAvg::update(dynamicgraph::Vector& res,
return res;
}
-
-
- } /* namespace sot */
+} /* namespace sot */
} /* namespace dynamicgraph */
diff --git a/src/tools/gradient-ascent.cpp b/src/tools/gradient-ascent.cpp
index 6abb2f4c..b8d1f872 100644
--- a/src/tools/gradient-ascent.cpp
+++ b/src/tools/gradient-ascent.cpp
@@ -21,46 +21,39 @@ namespace dg = ::dynamicgraph;
/* ---------------------------------------------------------------------------*/
namespace dynamicgraph {
- namespace sot {
-
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GradientAscent,"GradientAscent");
+namespace sot {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GradientAscent, "GradientAscent");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-GradientAscent::
-GradientAscent( const std::string& n )
- :Entity(n)
- ,gradientSIN(NULL, "GradientAscent(" + n + ")::input(vector)::gradient")
- ,learningRateSIN(NULL, "GradientAscent(" + n + ")::input(double)::learningRate")
- ,refresherSINTERN( "GradientAscent("+n+")::intern(dummy)::refresher" )
- ,valueSOUT(
- boost::bind(&GradientAscent::update,this,_1,_2),
- gradientSIN << refresherSINTERN, "GradientAscent(" + n + ")::output(vector)::value")
- ,init(false)
-{
+GradientAscent::GradientAscent(const std::string &n)
+ : Entity(n),
+ gradientSIN(NULL, "GradientAscent(" + n + ")::input(vector)::gradient"),
+ learningRateSIN(NULL,
+ "GradientAscent(" + n + ")::input(double)::learningRate"),
+ refresherSINTERN("GradientAscent(" + n + ")::intern(dummy)::refresher"),
+ valueSOUT(boost::bind(&GradientAscent::update, this, _1, _2),
+ gradientSIN << refresherSINTERN,
+ "GradientAscent(" + n + ")::output(vector)::value"),
+ init(false) {
// Register signals into the entity.
signalRegistration(gradientSIN << learningRateSIN << valueSOUT);
- refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
+ refresherSINTERN.setDependencyType(TimeDependency<int>::ALWAYS_READY);
}
-GradientAscent::~GradientAscent()
-{
-}
+GradientAscent::~GradientAscent() {}
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
-dynamicgraph::Vector& GradientAscent::update(dynamicgraph::Vector& res,
- const int& inTime)
-{
- const dynamicgraph::Vector& gradient = gradientSIN(inTime);
- const double& learningRate = learningRateSIN(inTime);
+dynamicgraph::Vector &GradientAscent::update(dynamicgraph::Vector &res,
+ const int &inTime) {
+ const dynamicgraph::Vector &gradient = gradientSIN(inTime);
+ const double &learningRate = learningRateSIN(inTime);
if (init == false) {
init = true;
@@ -74,5 +67,5 @@ dynamicgraph::Vector& GradientAscent::update(dynamicgraph::Vector& res,
return res;
}
- } /* namespace sot */
+} /* namespace sot */
} /* namespace dynamicgraph */
diff --git a/src/tools/gripper-control.cpp b/src/tools/gripper-control.cpp
index 363ca3db..ef947b93 100644
--- a/src/tools/gripper-control.cpp
+++ b/src/tools/gripper-control.cpp
@@ -9,94 +9,81 @@
#define ENABLE_RT_LOG
-#include <sot/core/gripper-control.hh>
#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/real-time-logger.h>
#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/real-time-logger.h>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GripperControlPlugin,"GripperControl");
-
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GripperControlPlugin, "GripperControl");
/* --- PLUGIN --------------------------------------------------------------- */
/* --- 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;
// TODO: hard coded
const double DT = 0.005;
-GripperControl::
-GripperControl(void)
- :offset( GripperControl::OFFSET_DEFAULT )
- ,factor()
-{}
-
-GripperControlPlugin::
-GripperControlPlugin( const std::string & name )
- :Entity(name)
- ,calibrationStarted(false)
- ,positionSIN(NULL,"GripperControl("+name+")::input(vector)::position")
- ,positionDesSIN(NULL,"GripperControl("+name+")::input(vector)::positionDes")
- ,torqueSIN(NULL,"GripperControl("+name+")::input(vector)::torque")
- ,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( torqueLimit )
- ,desiredPositionSOUT( SOT_MEMBER_SIGNAL_4( GripperControl::computeDesiredPosition,
- positionSIN,dynamicgraph::Vector,
- positionDesSIN,dynamicgraph::Vector,
- torqueSIN,dynamicgraph::Vector,
- torqueLimitSIN,dynamicgraph::Vector ),
- "GripperControl("+name+")::output(vector)::reference" )
-{
+GripperControl::GripperControl(void)
+ : offset(GripperControl::OFFSET_DEFAULT), factor() {}
+
+GripperControlPlugin::GripperControlPlugin(const std::string &name)
+ : Entity(name), calibrationStarted(false),
+ positionSIN(NULL,
+ "GripperControl(" + name + ")::input(vector)::position"),
+ positionDesSIN(NULL, "GripperControl(" + name +
+ ")::input(vector)::positionDes"),
+ torqueSIN(NULL, "GripperControl(" + name + ")::input(vector)::torque"),
+ 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(torqueLimit),
+ desiredPositionSOUT(
+ SOT_MEMBER_SIGNAL_4(GripperControl::computeDesiredPosition,
+ positionSIN, dynamicgraph::Vector, positionDesSIN,
+ dynamicgraph::Vector, torqueSIN,
+ dynamicgraph::Vector, torqueLimitSIN,
+ dynamicgraph::Vector),
+ "GripperControl(" + name + ")::output(vector)::reference") {
sotDEBUGIN(5);
- positionSIN.plug( &positionReduceSOUT );
- torqueSIN.plug( &torqueReduceSOUT );
- torqueLimitSIN.plug( &torqueLimitReduceSOUT );
+ positionSIN.plug(&positionReduceSOUT);
+ torqueSIN.plug(&torqueReduceSOUT);
+ torqueLimitSIN.plug(&torqueLimitReduceSOUT);
- signalRegistration( positionSIN << positionDesSIN
- << torqueSIN << torqueLimitSIN << selectionSIN
- << desiredPositionSOUT
- << positionFullSizeSIN
- << torqueFullSizeSIN
- << torqueLimitFullSizeSIN);
+ signalRegistration(
+ positionSIN << positionDesSIN << torqueSIN << torqueLimitSIN
+ << selectionSIN << desiredPositionSOUT << positionFullSizeSIN
+ << torqueFullSizeSIN << torqueLimitFullSizeSIN);
sotDEBUGOUT(5);
initCommands();
}
+GripperControlPlugin::~GripperControlPlugin(void) {}
-GripperControlPlugin::
-~GripperControlPlugin( void )
-{}
-
-std::string GripperControlPlugin::
-getDocString () const
-{
- std::string docstring ="Control of gripper.";
+std::string GripperControlPlugin::getDocString() const {
+ std::string docstring = "Control of gripper.";
return docstring;
}
@@ -104,52 +91,54 @@ getDocString () const
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
-void GripperControl::
-computeIncrement( const dynamicgraph::Vector& torques,
- const dynamicgraph::Vector& torqueLimits,
- const dynamicgraph::Vector& currentNormVel )
-{
+void GripperControl::computeIncrement(
+ const dynamicgraph::Vector &torques,
+ const dynamicgraph::Vector &torqueLimits,
+ const dynamicgraph::Vector ¤tNormVel) {
const dynamicgraph::Vector::Index SIZE = currentNormVel.size();
// initialize factor, if needed.
- if( factor.size()!=SIZE ) { factor.resize(SIZE); factor.fill(1.); }
+ if (factor.size() != SIZE) {
+ factor.resize(SIZE);
+ factor.fill(1.);
+ }
// Torque not provided?
- if (torques.size() == 0)
- {
+ if (torques.size() == 0) {
dgRTLOG() << "torque is not provided " << std::endl;
return;
}
- for( int i=0;i<SIZE;++i )
- {
+ for (int i = 0; i < SIZE; ++i) {
// apply a reduction factor if the torque limits are exceeded
// and the velocity goes in the same way
- if( (torques(i)>torqueLimits(i))&&(currentNormVel(i)>0) )
- { factor(i)*=offset; }
- else if( (torques(i)< -torqueLimits(i))&&(currentNormVel(i)<0) )
- { factor(i)*=offset; }
+ if ((torques(i) > torqueLimits(i)) && (currentNormVel(i) > 0)) {
+ factor(i) *= offset;
+ } else if ((torques(i) < -torqueLimits(i)) && (currentNormVel(i) < 0)) {
+ factor(i) *= offset;
+ }
// otherwise, release smoothly the reduction if possible/needed
- else { factor(i)/=offset; }
+ else {
+ factor(i) /= offset;
+ }
// ensure factor is in )0,1(
- factor(i) = std::min(1., std::max(factor(i),0.));
+ factor(i) = std::min(1., std::max(factor(i), 0.));
}
}
-dynamicgraph::Vector& GripperControl::
-computeDesiredPosition( const dynamicgraph::Vector& currentPos,
- 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 ... */ }
// compute the desired velocity
- dynamicgraph::Vector velocity = (desiredPos - currentPos)* (1. / DT);
+ dynamicgraph::Vector velocity = (desiredPos - currentPos) * (1. / DT);
computeIncrement(torques, torqueLimits, velocity);
@@ -167,40 +156,38 @@ computeDesiredPosition( const dynamicgraph::Vector& currentPos,
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++; }
+ for (int i = 0; i < fullsize.size(); ++i) {
+ if (selec(i))
+ size++;
+ }
- int curs=0;
+ int curs = 0;
desPos.resize(size);
- for( int i=0;i<fullsize.size();++i )
- { if( selec(i) ) desPos(curs++)=fullsize(i); }
+ for (int i = 0; i < fullsize.size(); ++i) {
+ if (selec(i))
+ desPos(curs++) = fullsize(i);
+ }
return desPos;
}
-
/* --- COMMANDLINE ---------------------------------------------------------- */
/* --- COMMANDLINE ---------------------------------------------------------- */
/* --- COMMANDLINE ---------------------------------------------------------- */
-void GripperControlPlugin::initCommands()
-{
+void GripperControlPlugin::initCommands() {
namespace dc = ::dynamicgraph::command;
addCommand("offset",
- dc::makeCommandVoid1(*this,&GripperControlPlugin::setOffset,
- "set the offset (should be in )0, 1( )."));
+ dc::makeCommandVoid1(*this, &GripperControlPlugin::setOffset,
+ "set the offset (should be in )0, 1( )."));
}
-
-void GripperControlPlugin::setOffset(const double & value)
-{
- if( (value>0)&&(value<1) ) offset = value;
- else throw std::invalid_argument ("The offset should be in )0, 1(.");
+void GripperControlPlugin::setOffset(const double &value) {
+ if ((value > 0) && (value < 1))
+ offset = value;
+ else
+ throw std::invalid_argument("The offset should be in )0, 1(.");
}
diff --git a/src/tools/joint-limitator.cpp b/src/tools/joint-limitator.cpp
index 42d7d685..b4eec1b8 100644
--- a/src/tools/joint-limitator.cpp
+++ b/src/tools/joint-limitator.cpp
@@ -7,92 +7,84 @@
*
*/
-#include <sot/core/joint-limitator.hh>
-#include <sot/core/exception-feature.hh>
#include <sot/core/debug.hh>
+#include <sot/core/exception-feature.hh>
#include <sot/core/factory.hh>
-
+#include <sot/core/joint-limitator.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointLimitator,"JointLimitator");
-
-JointLimitator::
-JointLimitator( const string& fName )
- : Entity( fName )
- ,jointSIN( NULL,"JointLimitator("+name+")::input(vector)::joint" )
- ,upperJlSIN( NULL,"JointLimitator("+name+")::input(vector)::upperJl" )
- ,lowerJlSIN( NULL,"JointLimitator("+name+")::input(vector)::lowerJl" )
- ,controlSIN( NULL,"JointLimitator("+name+")::input(vector)::controlIN" )
- ,controlSOUT( boost::bind(&JointLimitator::computeControl,this,_1,_2),
- jointSIN<<upperJlSIN<<lowerJlSIN<<controlSIN,
- "JointLimitator("+name+")::output(vector)::control" )
- ,widthJlSINTERN( boost::bind(&JointLimitator::computeWidthJl,this,_1,_2),
- upperJlSIN<<lowerJlSIN,
- "JointLimitator("+name+")::input(vector)::widthJl" )
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointLimitator, "JointLimitator");
+
+JointLimitator::JointLimitator(const string &fName)
+ : Entity(fName),
+ jointSIN(NULL, "JointLimitator(" + name + ")::input(vector)::joint"),
+ upperJlSIN(NULL, "JointLimitator(" + name + ")::input(vector)::upperJl"),
+ lowerJlSIN(NULL, "JointLimitator(" + name + ")::input(vector)::lowerJl"),
+ controlSIN(NULL,
+ "JointLimitator(" + name + ")::input(vector)::controlIN"),
+ controlSOUT(boost::bind(&JointLimitator::computeControl, this, _1, _2),
+ jointSIN << upperJlSIN << lowerJlSIN << controlSIN,
+ "JointLimitator(" + name + ")::output(vector)::control"),
+ widthJlSINTERN(boost::bind(&JointLimitator::computeWidthJl, this, _1, _2),
+ upperJlSIN << lowerJlSIN,
+ "JointLimitator(" + name + ")::input(vector)::widthJl")
{
- signalRegistration( jointSIN<<upperJlSIN<<lowerJlSIN
- <<controlSIN<<controlSOUT<<widthJlSINTERN );
+ signalRegistration(jointSIN << upperJlSIN << lowerJlSIN << controlSIN
+ << controlSOUT << widthJlSINTERN);
}
-dynamicgraph::Vector&
-JointLimitator::computeWidthJl (dynamicgraph::Vector& res,const int& time)
-{
+dynamicgraph::Vector &JointLimitator::computeWidthJl(dynamicgraph::Vector &res,
+ const int &time) {
sotDEBUGIN(15);
const dynamicgraph::Vector UJL = upperJlSIN.access(time);
const dynamicgraph::Vector LJL = lowerJlSIN.access(time);
- const dynamicgraph::Vector::Index SIZE=UJL.size();
+ const dynamicgraph::Vector::Index SIZE = UJL.size();
res.resize(SIZE);
- for( unsigned int i=0;i<SIZE;++i )
- { res(i)=UJL(i)-LJL(i); }
+ for (unsigned int i = 0; i < SIZE; ++i) {
+ res(i) = UJL(i) - LJL(i);
+ }
sotDEBUGOUT(15);
return res;
}
-
-dynamicgraph::Vector&
-JointLimitator::computeControl (dynamicgraph::Vector& uOUT,int time)
-{
+dynamicgraph::Vector &JointLimitator::computeControl(dynamicgraph::Vector &uOUT,
+ int time) {
sotDEBUGIN(15);
- const dynamicgraph::Vector& q = jointSIN.access(time);
- const dynamicgraph::Vector& UJL = upperJlSIN.access(time);
- const dynamicgraph::Vector& LJL = lowerJlSIN.access(time);
- const dynamicgraph::Vector& uIN = controlSIN.access(time);
+ const dynamicgraph::Vector &q = jointSIN.access(time);
+ const dynamicgraph::Vector &UJL = upperJlSIN.access(time);
+ const dynamicgraph::Vector &LJL = lowerJlSIN.access(time);
+ const dynamicgraph::Vector &uIN = controlSIN.access(time);
dynamicgraph::Vector::Index controlSize = uIN.size();
- uOUT.resize(controlSize); uOUT.setZero();
+ uOUT.resize(controlSize);
+ uOUT.setZero();
dynamicgraph::Vector::Index offset = q.size() - uIN.size();
assert(offset >= 0);
- 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
- ((qnext<UJL(i+offset))&&(qnext>LJL(i+offset)))) {
- uOUT(i)=uIN(i);
- }
- sotDEBUG(25) << i
- << ": " <<qnext<<" in? [" << LJL(i)<<","<<UJL(i)<<"]"<<endl;
+ 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
+ ((qnext < UJL(i + offset)) && (qnext > LJL(i + offset)))) {
+ uOUT(i) = uIN(i);
}
+ sotDEBUG(25) << i << ": " << qnext << " in? [" << LJL(i) << "," << UJL(i)
+ << "]" << endl;
+ }
sotDEBUGOUT(15);
return uOUT;
}
+void JointLimitator::display(std::ostream &os) const {
-void JointLimitator::
-display( std::ostream& os ) const
-{
-
-
- os <<"JointLimitator <"<<name<<"> ... TODO";
-
+ os << "JointLimitator <" << name << "> ... TODO";
}
diff --git a/src/tools/joint-trajectory-command.hh b/src/tools/joint-trajectory-command.hh
index fb9fdc16..c645e59d 100644
--- a/src/tools/joint-trajectory-command.hh
+++ b/src/tools/joint-trajectory-command.hh
@@ -10,40 +10,36 @@
#include <boost/assign/list_of.hpp>
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command.h>
#include <sot/core/joint-trajectory-entity.hh>
-namespace dynamicgraph{ namespace sot {
- namespace command { namespace classSot {
- using ::dynamicgraph::command::Command;
- using ::dynamicgraph::command::Value;
-
- class SetInitTrajectory : public Command
- {
- public:
- virtual ~SetInitTrajectory() {}
-
- /// Set the initial trajectory.
- SetInitTrajectory(SotJointTrajectoryEntity & entity,
- const std::string &docstring) :
- 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 sot {
+namespace command {
+namespace classSot {
+using ::dynamicgraph::command::Command;
+using ::dynamicgraph::command::Value;
+
+class SetInitTrajectory : public Command {
+public:
+ virtual ~SetInitTrajectory() {}
+
+ /// Set the initial trajectory.
+ SetInitTrajectory(SotJointTrajectoryEntity &entity,
+ const std::string &docstring)
+ : Command(entity, boost::assign::list_of(Value::STRING), docstring) {}
+
+ virtual Value doExecute() {
+
+ // return void
+ return Value();
+ }
+};
+} // 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 d76d568f..2bcd6b7f 100644
--- a/src/tools/joint-trajectory-entity.cpp
+++ b/src/tools/joint-trajectory-entity.cpp
@@ -10,17 +10,16 @@
#include <sot/core/debug.hh>
#include <sot/core/matrix-geometry.hh>
#ifdef VP_DEBUG
- class sotJTE__INIT
- {
- public:sotJTE__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
- };
- sotJTE__INIT sotJTE_initiator;
+class sotJTE__INIT {
+public:
+ sotJTE__INIT(void) { dynamicgraph::sot::DebugTrace::openFile(); }
+};
+sotJTE__INIT sotJTE_initiator;
#endif //#ifdef VP_DEBUG
-
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/command-bind.h>
+#include <dynamic-graph/factory.h>
#include <sot/core/joint-trajectory-entity.hh>
@@ -31,88 +30,77 @@ using namespace dynamicgraph;
using namespace dynamicgraph::sot;
using namespace dynamicgraph::command;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotJointTrajectoryEntity,"SotJointTrajectoryEntity");
-
-
-SotJointTrajectoryEntity::
-SotJointTrajectoryEntity(const std::string &n):
- Entity(n),
- refresherSINTERN("SotJointTrajectoryEntity("+n+")::intern(dummy)::refresher"),
- OneStepOfUpdateS(boost::bind(&SotJointTrajectoryEntity::OneStepOfUpdate,this,_1,_2),
- refresherSINTERN << trajectorySIN,
- "SotJointTrajectory("+n+")::onestepofupdate"),
- positionSOUT(boost::bind(&SotJointTrajectoryEntity::getNextPosition,
- this,_1,_2),
- OneStepOfUpdateS,
- "SotJointTrajectory("+n+")::output(vector)::position"),
- comSOUT(boost::bind(&SotJointTrajectoryEntity::getNextCoM,
- this,_1,_2),
- OneStepOfUpdateS,
- "SotJointTrajectory("+n+")::output(vector)::com"),
- zmpSOUT(boost::bind(&SotJointTrajectoryEntity::getNextCoP,
- this,_1,_2),
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotJointTrajectoryEntity,
+ "SotJointTrajectoryEntity");
+
+SotJointTrajectoryEntity::SotJointTrajectoryEntity(const std::string &n)
+ : Entity(n), refresherSINTERN("SotJointTrajectoryEntity(" + n +
+ ")::intern(dummy)::refresher"),
+ OneStepOfUpdateS(
+ boost::bind(&SotJointTrajectoryEntity::OneStepOfUpdate, this, _1, _2),
+ refresherSINTERN << trajectorySIN,
+ "SotJointTrajectory(" + n + ")::onestepofupdate"),
+ positionSOUT(
+ boost::bind(&SotJointTrajectoryEntity::getNextPosition, this, _1, _2),
+ OneStepOfUpdateS,
+ "SotJointTrajectory(" + n + ")::output(vector)::position"),
+ comSOUT(boost::bind(&SotJointTrajectoryEntity::getNextCoM, this, _1, _2),
+ OneStepOfUpdateS,
+ "SotJointTrajectory(" + n + ")::output(vector)::com"),
+ zmpSOUT(boost::bind(&SotJointTrajectoryEntity::getNextCoP, this, _1, _2),
+ OneStepOfUpdateS,
+ "SotJointTrajectory(" + n + ")::output(vector)::zmp"),
+ waistSOUT(
+ boost::bind(&SotJointTrajectoryEntity::getNextWaist, this, _1, _2),
OneStepOfUpdateS,
- "SotJointTrajectory("+n+")::output(vector)::zmp"),
- waistSOUT(boost::bind(&SotJointTrajectoryEntity::getNextWaist,
- this,_1,_2),
- OneStepOfUpdateS,
- "SotJointTrajectory("+n+")::output(MatrixHomogeneous)::waist"),
- seqIdSOUT(boost::bind(&SotJointTrajectoryEntity::getSeqId,
- this,_1,_2),
- OneStepOfUpdateS,
- "SotJointTrajectory("+n+")::output(uint)::seqid"),
- trajectorySIN(NULL,"SotJointTrajectory("+n+")::input(trajectory)::trajectoryIN"),
- index_(0),
- traj_timestamp_(0,0),
- seqid_(0),
- deque_traj_(0)
-{
+ "SotJointTrajectory(" + n + ")::output(MatrixHomogeneous)::waist"),
+ seqIdSOUT(boost::bind(&SotJointTrajectoryEntity::getSeqId, this, _1, _2),
+ OneStepOfUpdateS,
+ "SotJointTrajectory(" + n + ")::output(uint)::seqid"),
+ trajectorySIN(NULL, "SotJointTrajectory(" + n +
+ ")::input(trajectory)::trajectoryIN"),
+ index_(0), traj_timestamp_(0, 0), seqid_(0), deque_traj_(0) {
using namespace command;
sotDEBUGIN(5);
-
- signalRegistration( positionSOUT << comSOUT << zmpSOUT
- << waistSOUT << seqIdSOUT << trajectorySIN);
- refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
+ signalRegistration(positionSOUT << comSOUT << zmpSOUT << waistSOUT
+ << seqIdSOUT << trajectorySIN);
+ refresherSINTERN.setDependencyType(TimeDependency<int>::ALWAYS_READY);
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,
+ makeCommandVoid1(*this, &SotJointTrajectoryEntity::setInitTraj,
docCommandVoid1("Set initial trajectory",
"string (trajectory)")));
sotDEBUGOUT(5);
}
-void SotJointTrajectoryEntity::UpdatePoint(const JointTrajectoryPoint &aJTP)
-{
+void SotJointTrajectoryEntity::UpdatePoint(const JointTrajectoryPoint &aJTP) {
sotDEBUGIN(5);
// Posture
- std::vector<JointTrajectoryPoint>::size_type possize =
- aJTP.positions_.size();
- if (possize==0)
+ std::vector<JointTrajectoryPoint>::size_type possize = aJTP.positions_.size();
+ if (possize == 0)
return;
pose_.resize(aJTP.positions_.size());
- for(std::vector<JointTrajectoryPoint>::size_type i=0;
- i<possize-5;i++)
- {
- pose_(i) = aJTP.positions_[i];
- sotDEBUG(5) << pose_(i) << " " << std::endl;
- }
+ for (std::vector<JointTrajectoryPoint>::size_type i = 0; i < possize - 5;
+ i++) {
+ pose_(i) = aJTP.positions_[i];
+ sotDEBUG(5) << pose_(i) << " " << std::endl;
+ }
// Center of Mass
com_.resize(3);
- for(std::vector<JointTrajectoryPoint>::size_type i=possize-5,j=0;
- i<possize-2;i++,j++)
+ for (std::vector<JointTrajectoryPoint>::size_type i = possize - 5, j = 0;
+ i < possize - 2; i++, j++)
com_(j) = aJTP.positions_[i];
sotDEBUG(5) << "com: " << com_ << std::endl;
@@ -130,53 +118,49 @@ void SotJointTrajectoryEntity::UpdatePoint(const JointTrajectoryPoint &aJTP)
sotDEBUG(5) << "waist: " << waist_ << std::endl;
// Center of Pressure
cop_.resize(3);
- for(std::vector<JointTrajectoryPoint>::size_type i=possize-2,j=0;
- i<possize;i++,j++)
+ for (std::vector<JointTrajectoryPoint>::size_type i = possize - 2, j = 0;
+ i < possize; i++, j++)
cop_(j) = aJTP.positions_[i];
- cop_(2)=-0.055;
+ cop_(2) = -0.055;
sotDEBUG(5) << "cop_: " << cop_ << std::endl;
- sotDEBUGOUT(5) ;
+ sotDEBUGOUT(5);
}
-void SotJointTrajectoryEntity::UpdateTrajectory(const Trajectory &aTrajectory)
-{
+void SotJointTrajectoryEntity::UpdateTrajectory(const Trajectory &aTrajectory) {
sotDEBUGIN(3);
sotDEBUG(3) << "traj_timestamp: " << traj_timestamp_
- << " aTrajectory.header_.stamp_" << aTrajectory.header_.stamp_ ;
+ << " aTrajectory.header_.stamp_" << aTrajectory.header_.stamp_;
// Do we have the same trajectory, or are we at the initialization phase ?
- if ((traj_timestamp_==aTrajectory.header_.stamp_) && (deque_traj_.size()!=0))
+ if ((traj_timestamp_ == aTrajectory.header_.stamp_) &&
+ (deque_traj_.size() != 0))
index_++;
- else
- {
- // No we have a new trajectory.
- sotDEBUG(3) << "index: " << index_
- << " aTrajectory.points_.size(): " << aTrajectory.points_.size();
-
- // Put the new trajectory in the queue
-
- // if there is nothing
- if (deque_traj_.size()==0)
- {
- deque_traj_.push_back(aTrajectory);
- index_=0;
- }
- else
- {
- index_++;
- // if it is not already inside the queue.
- if (deque_traj_.back().header_.stamp_ == aTrajectory.header_.stamp_) {}
- else deque_traj_.push_back(aTrajectory);
- }
+ else {
+ // No we have a new trajectory.
+ sotDEBUG(3) << "index: " << index_ << " aTrajectory.points_.size(): "
+ << aTrajectory.points_.size();
+
+ // Put the new trajectory in the queue
+
+ // if there is nothing
+ if (deque_traj_.size() == 0) {
+ deque_traj_.push_back(aTrajectory);
+ index_ = 0;
+ } else {
+ index_++;
+ // if it is not already inside the queue.
+ if (deque_traj_.back().header_.stamp_ == aTrajectory.header_.stamp_) {
+ } else
+ deque_traj_.push_back(aTrajectory);
}
-
-
+ }
sotDEBUG(3) << "step 1 " << std::endl
<< "header: " << std::endl
<< " timestamp:"
<< static_cast<double>(aTrajectory.header_.stamp_.secs_) +
- 0.000000001 * static_cast<double>(aTrajectory.header_.stamp_.nsecs_)
+ 0.000000001 *
+ static_cast<double>(aTrajectory.header_.stamp_.nsecs_)
<< " seq:" << aTrajectory.header_.seq_ << " "
<< " frame_id:" << aTrajectory.header_.frame_id_
<< " index_: " << index_
@@ -184,59 +168,51 @@ void SotJointTrajectoryEntity::UpdateTrajectory(const Trajectory &aTrajectory)
<< std::endl;
// 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;
- }
-
- // If the new trajectory has a problem
- if (deque_traj_.front().points_.size()==0)
- {
- // then neutralize the entity
- index_=0;
- sotDEBUG(3) << "current_traj_.points_.size()="
- << deque_traj_.front().points_.size() << std::endl;
- return;
- }
-
- // Strategy at the end of the trajectory when no new information is available:
- // It is assumed that the last pose is balanced, and we keep providing
- // this pose to the robot.
- if ((index_!=0) && (deque_traj_.size()==1))
- {
- index_= deque_traj_.front().points_.size()-1;
- }
- sotDEBUG(3) << "index_=current_traj_.points_.size()-1;" << std::endl;
+ if (index_ >= deque_traj_.front().points_.size()) {
+
+ if (deque_traj_.size() > 1) {
+ deque_traj_.pop_front();
+ index_ = 0;
}
- sotDEBUG(3) << "index_:" << index_
- << " current_traj_.points_.size():" << deque_traj_.front().points_.size()
- << std::endl;
+ // If the new trajectory has a problem
+ if (deque_traj_.front().points_.size() == 0) {
+ // then neutralize the entity
+ index_ = 0;
+ sotDEBUG(3) << "current_traj_.points_.size()="
+ << deque_traj_.front().points_.size() << std::endl;
+ return;
+ }
+
+ // Strategy at the end of the trajectory when no new information is
+ // available: It is assumed that the last pose is balanced, and we keep
+ // providing this pose to the robot.
+ if ((index_ != 0) && (deque_traj_.size() == 1)) {
+ index_ = deque_traj_.front().points_.size() - 1;
+ }
+ sotDEBUG(3) << "index_=current_traj_.points_.size()-1;" << std::endl;
+ }
+
+ sotDEBUG(3) << "index_:" << index_ << " current_traj_.points_.size():"
+ << deque_traj_.front().points_.size() << std::endl;
seqid_ = deque_traj_.front().header_.seq_;
UpdatePoint(deque_traj_.front().points_[index_]);
sotDEBUGOUT(3);
}
-int & SotJointTrajectoryEntity::OneStepOfUpdate(int &dummy,const int & time)
-{
+int &SotJointTrajectoryEntity::OneStepOfUpdate(int &dummy, const int &time) {
sotDEBUGIN(4);
- const Trajectory & atraj = trajectorySIN(time);
- if ((atraj.header_.stamp_.secs_!=deque_traj_.front().header_.stamp_.secs_) ||
- (atraj.header_.stamp_.nsecs_!=deque_traj_.front().header_.stamp_.nsecs_))
- {
- if (index_ < deque_traj_.front().points_.size()-1)
- {
- sotDEBUG(4) << "Overwrite trajectory without completion."
- << index_ << " "
- << deque_traj_.front().points_.size()
- <<std::endl;
- }
+ const Trajectory &atraj = trajectorySIN(time);
+ if ((atraj.header_.stamp_.secs_ !=
+ deque_traj_.front().header_.stamp_.secs_) ||
+ (atraj.header_.stamp_.nsecs_ !=
+ deque_traj_.front().header_.stamp_.nsecs_)) {
+ if (index_ < deque_traj_.front().points_.size() - 1) {
+ sotDEBUG(4) << "Overwrite trajectory without completion." << index_ << " "
+ << deque_traj_.front().points_.size() << std::endl;
}
+ }
sotDEBUG(4) << "Finished to read trajectorySIN" << std::endl;
UpdateTrajectory(atraj);
@@ -246,39 +222,35 @@ int & SotJointTrajectoryEntity::OneStepOfUpdate(int &dummy,const int & time)
return dummy;
}
-sot::MatrixHomogeneous
-SotJointTrajectoryEntity::
-XYZThetaToMatrixHomogeneous (const dynamicgraph::Vector& xyztheta)
-{
- assert (xyztheta.size () == 4);
- dynamicgraph::Vector t (3);
- t (0) = xyztheta (0);
- t (1) = xyztheta (1);
- t (2) = xyztheta (2);
+sot::MatrixHomogeneous SotJointTrajectoryEntity::XYZThetaToMatrixHomogeneous(
+ const dynamicgraph::Vector &xyztheta) {
+ assert(xyztheta.size() == 4);
+ dynamicgraph::Vector t(3);
+ t(0) = xyztheta(0);
+ t(1) = xyztheta(1);
+ t(2) = xyztheta(2);
Eigen::Affine3d trans;
trans = Eigen::Translation3d(t);
- Eigen::Affine3d _Rd(Eigen::AngleAxisd(xyztheta (3),Eigen::Vector3d::UnitZ()));
+ Eigen::Affine3d _Rd(Eigen::AngleAxisd(xyztheta(3), Eigen::Vector3d::UnitZ()));
sot::MatrixHomogeneous res;
- res = _Rd*trans;
+ res = _Rd * trans;
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_;
sotDEBUG(5) << pos;
- sotDEBUGOUT(5) ;
+ sotDEBUGOUT(5);
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_;
@@ -286,10 +258,9 @@ 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_;
@@ -297,10 +268,9 @@ 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_;
@@ -308,10 +278,8 @@ getNextWaist(sot::MatrixHomogeneous &waist,
return waist_;
}
-unsigned int &SotJointTrajectoryEntity::
-getSeqId(unsigned int &seqid,
- const int & time)
-{
+unsigned int &SotJointTrajectoryEntity::getSeqId(unsigned int &seqid,
+ const int &time) {
sotDEBUGIN(5);
OneStepOfUpdateS(time);
seqid = seqid_;
@@ -319,25 +287,19 @@ getSeqId(unsigned int &seqid,
return seqid;
}
-void SotJointTrajectoryEntity::
-loadFile(const std::string &)
-{
+void SotJointTrajectoryEntity::loadFile(const std::string &) {
sotDEBUGIN(5);
- //TODO
+ // TODO
sotDEBUGOUT(5);
}
-void SotJointTrajectoryEntity::
-display(std::ostream &os) const
-{
+void SotJointTrajectoryEntity::display(std::ostream &os) const {
sotDEBUGIN(5);
os << this;
sotDEBUGOUT(5);
}
-void SotJointTrajectoryEntity::
-setInitTraj(const std::string &as)
-{
+void SotJointTrajectoryEntity::setInitTraj(const std::string &as) {
sotDEBUGIN(5);
std::istringstream is(as);
init_traj_.deserialize(is);
diff --git a/src/tools/kalman.cpp b/src/tools/kalman.cpp
index 4954883f..adc5b936 100644
--- a/src/tools/kalman.cpp
+++ b/src/tools/kalman.cpp
@@ -10,187 +10,172 @@
*/
/* --- SOT --- */
-#include <sot/core/kalman.hh> /* Header of the class implemented here. */
+#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/factory.hh>
-#include <dynamic-graph/factory.h>
+#include <sot/core/kalman.hh> /* Header of the class implemented here. */
#include <dynamic-graph/command-setter.h>
namespace dynamicgraph {
- using command::Setter;
- namespace sot {
-
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Kalman,"Kalman");
-
- Kalman::Kalman( const std::string& name )
- : 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" )
- ,noiseMeasureSIN( NULL,"Kalman("+name+")::input(matrix)::R" )
-
- ,statePredictedSIN (0, "Kalman("+name+")::input(vector)::x_pred")
- ,observationPredictedSIN (0, "Kalman("+name+")::input(vector)::y_pred")
- ,varianceUpdateSOUT ("Kalman("+name+")::output(vector)::P")
- ,stateUpdateSOUT ("Kalman("+name+")::output(vector)::x_est"),
- stateEstimation_ (),
- stateVariance_ ()
- {
- sotDEBUGIN(15);
- varianceUpdateSOUT.setFunction
- (boost::bind(&Kalman::computeVarianceUpdate,
- this,_1,_2));
- stateUpdateSOUT.setFunction (boost::bind(&Kalman::computeStateUpdate,
- this, _1, _2));
-
- signalRegistration( measureSIN << observationPredictedSIN
- << modelTransitionSIN
- << modelMeasureSIN << noiseTransitionSIN
- << noiseMeasureSIN << statePredictedSIN
- << stateUpdateSOUT << varianceUpdateSOUT );
-
- 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";
- addCommand ("setInitialVariance",
- new Setter <Kalman, Matrix> (*this,
- &Kalman::setStateVariance,
- docstring));
- sotDEBUGOUT(15);
- }
-
- Matrix & Kalman::
- computeVarianceUpdate (Matrix& Pk_k,const int& time)
- {
- sotDEBUGIN(15);
- if (time == 0) {
- // First time return variance initial state
- Pk_k = stateVariance_;
- // Set dependency to input signals for latter computations
- varianceUpdateSOUT.addDependency (noiseTransitionSIN);
- varianceUpdateSOUT.addDependency (modelTransitionSIN);
- } else {
-
- const Matrix &Q = noiseTransitionSIN( time );
- const Matrix& R = noiseMeasureSIN (time);
- const Matrix &F = modelTransitionSIN( time );
- const Matrix& H = modelMeasureSIN (time);
- const Matrix &Pk_1_k_1 = stateVariance_;
-
- sotDEBUG(15) << "Q=" << Q << std::endl;
- sotDEBUG(15) << "R=" << R << std::endl;
- sotDEBUG(15) << "F=" << F << std::endl;
- sotDEBUG(15) << "H=" << H << std::endl;
- sotDEBUG(15) << "Pk_1_k_1=" << Pk_1_k_1 << std::endl;
-
- FP_ = F * Pk_1_k_1;
- Pk_k_1_ = FP_*F.transpose();
- Pk_k_1_ += Q;
-
- sotDEBUG(15) << "F " <<std::endl << F << std::endl;
- sotDEBUG(15) << "P_{k-1|k-1} " <<std::endl<< Pk_1_k_1 << std::endl;
- sotDEBUG(15) << "F^T " <<std::endl<< F.transpose() << std::endl;
- sotDEBUG(15) << "P_{k|k-1} " << std::endl << Pk_k_1_ << std::endl;
-
- S_ = H * Pk_k_1_ * H.transpose () + R;
- K_ = Pk_k_1_ * H.transpose () * S_.inverse ();
- Pk_k = Pk_k_1_ - K_ * H * Pk_k_1_;
-
- sotDEBUG (15) << "S_{k} " << std::endl << S_ << std::endl;
- sotDEBUG (15) << "K_{k} " << std::endl << K_ << std::endl;
- sotDEBUG (15) << "P_{k|k} " << std::endl << Pk_k << std::endl;
-
- sotDEBUGOUT(15);
-
- stateVariance_ = Pk_k;
- }
- return Pk_k;
- }
-
- // ^
- // z = y - h (x )
- // k k k|k-1
- //
- // ^ ^
- // x = x + K z
- // k|k k|k-1 k k
- //
- // T
- // S = H P H + R
- // k k k|k-1 k
- //
- // T -1
- // K = P H S
- // k k|k-1 k k
- //
- // P = (I - K H ) P
- // k|k k k k|k-1
-
- Vector& Kalman::
- computeStateUpdate (Vector& x_est,const int& time )
- {
- sotDEBUGIN(15);
- if (time == 0) {
- // First time return variance initial state
- x_est = stateEstimation_;
- // Set dependency to input signals for latter computations
- stateUpdateSOUT.addDependency (measureSIN);
- stateUpdateSOUT.addDependency (observationPredictedSIN);
- stateUpdateSOUT.addDependency (modelMeasureSIN);
- stateUpdateSOUT.addDependency (noiseTransitionSIN);
- stateUpdateSOUT.addDependency (noiseMeasureSIN);
- stateUpdateSOUT.addDependency (statePredictedSIN);
- stateUpdateSOUT.addDependency (varianceUpdateSOUT);
- } else {
- varianceUpdateSOUT.recompute (time);
- const Vector & x_pred = statePredictedSIN (time);
- const Vector & y_pred = observationPredictedSIN (time);
- const Vector & y = measureSIN (time);
-
- sotDEBUG(25) << "K_{k} = "<<std::endl<< K_ << std::endl;
- sotDEBUG(25) << "y = " << y << std::endl;
- sotDEBUG(25) << "h (\\hat{x}_{k|k-1}) = " << y_pred << std::endl;
- sotDEBUG(25) << "y = " << y << std::endl;
-
- // Innovation: z_ = y - Hx
- z_ = y - y_pred;
- //x_est = x_pred + (K*(y-(H*x_pred)));
- x_est = x_pred + K_ * z_;
-
- sotDEBUG(25) << "z_{k} = " << z_ << std::endl;
- sotDEBUG(25) << "x_{k|k} = " << x_est << std::endl;
-
- stateEstimation_ = x_est;
- }
- sotDEBUGOUT (15);
- return x_est;
- }
-
- /* -------------------------------------------------------------------------- */
- /* --- MODELE --------------------------------------------------------------- */
- /* -------------------------------------------------------------------------- */
-
- void Kalman::
- display( std::ostream& ) const
- {
- }
-
- } // namespace sot
+using command::Setter;
+namespace sot {
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Kalman, "Kalman");
+
+Kalman::Kalman(const std::string &name)
+ : 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"),
+ noiseMeasureSIN(NULL, "Kalman(" + name + ")::input(matrix)::R")
+
+ ,
+ statePredictedSIN(0, "Kalman(" + name + ")::input(vector)::x_pred"),
+ observationPredictedSIN(0, "Kalman(" + name + ")::input(vector)::y_pred"),
+ varianceUpdateSOUT("Kalman(" + name + ")::output(vector)::P"),
+ stateUpdateSOUT("Kalman(" + name + ")::output(vector)::x_est"),
+ stateEstimation_(), stateVariance_() {
+ sotDEBUGIN(15);
+ varianceUpdateSOUT.setFunction(
+ boost::bind(&Kalman::computeVarianceUpdate, this, _1, _2));
+ stateUpdateSOUT.setFunction(
+ boost::bind(&Kalman::computeStateUpdate, this, _1, _2));
+
+ signalRegistration(measureSIN << observationPredictedSIN << modelTransitionSIN
+ << modelMeasureSIN << noiseTransitionSIN
+ << noiseMeasureSIN << statePredictedSIN
+ << stateUpdateSOUT << varianceUpdateSOUT);
+
+ 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";
+ addCommand(
+ "setInitialVariance",
+ new Setter<Kalman, Matrix>(*this, &Kalman::setStateVariance, docstring));
+ sotDEBUGOUT(15);
+}
+
+Matrix &Kalman::computeVarianceUpdate(Matrix &Pk_k, const int &time) {
+ sotDEBUGIN(15);
+ if (time == 0) {
+ // First time return variance initial state
+ Pk_k = stateVariance_;
+ // Set dependency to input signals for latter computations
+ varianceUpdateSOUT.addDependency(noiseTransitionSIN);
+ varianceUpdateSOUT.addDependency(modelTransitionSIN);
+ } else {
+
+ const Matrix &Q = noiseTransitionSIN(time);
+ const Matrix &R = noiseMeasureSIN(time);
+ const Matrix &F = modelTransitionSIN(time);
+ const Matrix &H = modelMeasureSIN(time);
+ const Matrix &Pk_1_k_1 = stateVariance_;
+
+ sotDEBUG(15) << "Q=" << Q << std::endl;
+ sotDEBUG(15) << "R=" << R << std::endl;
+ sotDEBUG(15) << "F=" << F << std::endl;
+ sotDEBUG(15) << "H=" << H << std::endl;
+ sotDEBUG(15) << "Pk_1_k_1=" << Pk_1_k_1 << std::endl;
+
+ FP_ = F * Pk_1_k_1;
+ Pk_k_1_ = FP_ * F.transpose();
+ Pk_k_1_ += Q;
+
+ sotDEBUG(15) << "F " << std::endl << F << std::endl;
+ sotDEBUG(15) << "P_{k-1|k-1} " << std::endl << Pk_1_k_1 << std::endl;
+ sotDEBUG(15) << "F^T " << std::endl << F.transpose() << std::endl;
+ sotDEBUG(15) << "P_{k|k-1} " << std::endl << Pk_k_1_ << std::endl;
+
+ S_ = H * Pk_k_1_ * H.transpose() + R;
+ K_ = Pk_k_1_ * H.transpose() * S_.inverse();
+ Pk_k = Pk_k_1_ - K_ * H * Pk_k_1_;
+
+ sotDEBUG(15) << "S_{k} " << std::endl << S_ << std::endl;
+ sotDEBUG(15) << "K_{k} " << std::endl << K_ << std::endl;
+ sotDEBUG(15) << "P_{k|k} " << std::endl << Pk_k << std::endl;
+
+ sotDEBUGOUT(15);
+
+ stateVariance_ = Pk_k;
+ }
+ return Pk_k;
+}
+
+// ^
+// z = y - h (x )
+// k k k|k-1
+//
+// ^ ^
+// x = x + K z
+// k|k k|k-1 k k
+//
+// T
+// S = H P H + R
+// k k k|k-1 k
+//
+// T -1
+// K = P H S
+// k k|k-1 k k
+//
+// P = (I - K H ) P
+// k|k k k k|k-1
+
+Vector &Kalman::computeStateUpdate(Vector &x_est, const int &time) {
+ sotDEBUGIN(15);
+ if (time == 0) {
+ // First time return variance initial state
+ x_est = stateEstimation_;
+ // Set dependency to input signals for latter computations
+ stateUpdateSOUT.addDependency(measureSIN);
+ stateUpdateSOUT.addDependency(observationPredictedSIN);
+ stateUpdateSOUT.addDependency(modelMeasureSIN);
+ stateUpdateSOUT.addDependency(noiseTransitionSIN);
+ stateUpdateSOUT.addDependency(noiseMeasureSIN);
+ stateUpdateSOUT.addDependency(statePredictedSIN);
+ stateUpdateSOUT.addDependency(varianceUpdateSOUT);
+ } else {
+ varianceUpdateSOUT.recompute(time);
+ const Vector &x_pred = statePredictedSIN(time);
+ const Vector &y_pred = observationPredictedSIN(time);
+ const Vector &y = measureSIN(time);
+
+ sotDEBUG(25) << "K_{k} = " << std::endl << K_ << std::endl;
+ sotDEBUG(25) << "y = " << y << std::endl;
+ sotDEBUG(25) << "h (\\hat{x}_{k|k-1}) = " << y_pred << std::endl;
+ sotDEBUG(25) << "y = " << y << std::endl;
+
+ // Innovation: z_ = y - Hx
+ z_ = y - y_pred;
+ // x_est = x_pred + (K*(y-(H*x_pred)));
+ x_est = x_pred + K_ * z_;
+
+ sotDEBUG(25) << "z_{k} = " << z_ << std::endl;
+ sotDEBUG(25) << "x_{k|k} = " << x_est << std::endl;
+
+ stateEstimation_ = x_est;
+ }
+ sotDEBUGOUT(15);
+ return x_est;
+}
+
+/* -------------------------------------------------------------------------- */
+/* --- MODELE --------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+void Kalman::display(std::ostream &) const {}
+
+} // namespace sot
} // namespace dynamicgraph
/*!
diff --git a/src/tools/latch.cpp b/src/tools/latch.cpp
index a936b00e..1cc6369d 100644
--- a/src/tools/latch.cpp
+++ b/src/tools/latch.cpp
@@ -5,11 +5,11 @@
*
*/
-#include <sot/core/latch.hh>
#include <sot/core/factory.hh>
+#include <sot/core/latch.hh>
namespace dynamicgraph {
- namespace sot {
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (Latch, "Latch");
- } // namespace sot
+namespace sot {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Latch, "Latch");
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/tools/mailbox-vector.cpp b/src/tools/mailbox-vector.cpp
index cdd2861c..f82850e2 100644
--- a/src/tools/mailbox-vector.cpp
+++ b/src/tools/mailbox-vector.cpp
@@ -11,17 +11,19 @@
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
-#include <sot/core/mailbox.hxx>
#include <sot/core/mailbox-vector.hh>
+#include <sot/core/mailbox.hxx>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
// Explicit template specialization
#ifdef WIN32
-MailboxVector::MailboxVector( const std::string& name): Mailbox<dynamicgraph::Vector> (name){}
+MailboxVector::MailboxVector(const std::string &name)
+ : Mailbox<dynamicgraph::Vector>(name) {}
#else
MAILBOX_TEMPLATE_SPE(dynamicgraph::Vector)
#endif
-template<>DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MailboxVector,"Mailbox<Vector>");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MailboxVector, "Mailbox<Vector>");
diff --git a/src/tools/motion-period.cpp b/src/tools/motion-period.cpp
index 42aaf83e..71cf9fad 100644
--- a/src/tools/motion-period.cpp
+++ b/src/tools/motion-period.cpp
@@ -12,97 +12,95 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
-#include <sot/core/motion-period.hh>
-#include <sot/core/exception-feature.hh>
#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;
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MotionPeriod,"MotionPeriod");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MotionPeriod, "MotionPeriod");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-MotionPeriod::
-MotionPeriod( const string& fName )
- : Entity( fName )
- ,motionParams( 0 )
- ,motionSOUT( boost::bind(&MotionPeriod::computeMotion,this,_1,_2),
- sotNOSIGNAL,
- "MotionPeriod("+name+")::output(vector)::motion" )
-{
- signalRegistration( motionSOUT );
- motionSOUT.setNeedUpdateFromAllChildren( true );
+MotionPeriod::MotionPeriod(const string &fName)
+ : Entity(fName), motionParams(0),
+ motionSOUT(boost::bind(&MotionPeriod::computeMotion, this, _1, _2),
+ sotNOSIGNAL,
+ "MotionPeriod(" + name + ")::output(vector)::motion") {
+ signalRegistration(motionSOUT);
+ motionSOUT.setNeedUpdateFromAllChildren(true);
resize(0);
}
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-dynamicgraph::Vector&
-MotionPeriod::computeMotion( dynamicgraph::Vector& res,const int& time )
-{
+dynamicgraph::Vector &MotionPeriod::computeMotion(dynamicgraph::Vector &res,
+ const int &time) {
sotDEBUGIN(15);
- res.resize( size );
- for( unsigned int i=0;i<size;++i )
- {
- const sotMotionParam & p = motionParams[i];
- 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}
- }
+ res.resize(size);
+ for (unsigned int i = 0; i < size; ++i) {
+ const sotMotionParam &p = motionParams[i];
+ 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}
+ }
+ }
sotDEBUGOUT(15);
return res;
}
-void MotionPeriod::
-resize( const unsigned int & _size )
-{
+void MotionPeriod::resize(const unsigned int &_size) {
size = _size;
motionParams.resize(size);
- for( unsigned int i=0;i<size;++i )
- {
- motionParams[i] .motionType = MOTION_CONSTANT;
- motionParams[i] .amplitude = 0;
- motionParams[i] .initPeriod = 0;
- motionParams[i] .period = 1;
- motionParams[i] .initAmplitude = 0;
- }
+ for (unsigned int i = 0; i < size; ++i) {
+ motionParams[i].motionType = MOTION_CONSTANT;
+ motionParams[i].amplitude = 0;
+ motionParams[i].initPeriod = 0;
+ motionParams[i].period = 1;
+ motionParams[i].initAmplitude = 0;
+ }
}
-
-void MotionPeriod::
-display( std::ostream& os ) const
-{
- os <<"MotionPeriod <"<<name<<"> ... TODO";
+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 4b868085..a7539a25 100644
--- a/src/tools/neck-limitation.cpp
+++ b/src/tools/neck-limitation.cpp
@@ -7,49 +7,43 @@
*
*/
-#include <sot/core/neck-limitation.hh>
+#include <dynamic-graph/pool.h>
#include <sot/core/debug.hh>
-#include <sot/core/factory.hh>
#include <sot/core/exception-tools.hh>
+#include <sot/core/factory.hh>
+#include <sot/core/neck-limitation.hh>
#include <sot/core/sot.hh>
-#include <dynamic-graph/pool.h>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NeckLimitation,"NeckLimitation");
+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::SIGN_TILT_DEFAULT = 1 ;
+const double NeckLimitation::COEFF_LINEAR_DEFAULT = -25.0 / 42.0;
+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 )
- ,coeffLinearPan( COEFF_LINEAR_DEFAULT )
- ,coeffAffinePan( COEFF_AFFINE_DEFAULT )
- ,signTilt( SIGN_TILT_DEFAULT )
-
- ,jointSIN( NULL,"NeckLimitation("+name+")::input(vector)::joint" )
- ,jointSOUT( boost::bind(&NeckLimitation::computeJointLimitation,this,_1,_2),
- jointSIN,
- "NeckLimitation("+name+")::output(dummy)::jointLimited" )
-{
+NeckLimitation::NeckLimitation(const std::string &name)
+ : Entity(name), panRank(PAN_RANK_DEFAULT), tiltRank(TILT_RANK_DEFAULT),
+ coeffLinearPan(COEFF_LINEAR_DEFAULT),
+ coeffAffinePan(COEFF_AFFINE_DEFAULT), signTilt(SIGN_TILT_DEFAULT)
+
+ ,
+ jointSIN(NULL, "NeckLimitation(" + name + ")::input(vector)::joint"),
+ jointSOUT(
+ boost::bind(&NeckLimitation::computeJointLimitation, this, _1, _2),
+ jointSIN,
+ "NeckLimitation(" + name + ")::output(dummy)::jointLimited") {
sotDEBUGIN(5);
- signalRegistration( jointSIN<<jointSOUT );
+ signalRegistration(jointSIN << jointSOUT);
sotDEBUGOUT(5);
}
-
-NeckLimitation::
-~NeckLimitation( void )
-{
+NeckLimitation::~NeckLimitation(void) {
sotDEBUGIN(5);
sotDEBUGOUT(5);
@@ -60,79 +54,78 @@ NeckLimitation::
/* --- 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 );
- jointLimited=joint;
-
- const double & pan = joint(panRank);
- const double & tilt = joint(tiltRank);
- double & panLimited = jointLimited(panRank);
- double & tiltLimited = jointLimited(tiltRank);
-
- if( fabs(pan)<1e-3 ) // pan == 0
- {
- sotDEBUG(15) << "Pan = 0" << std::endl;
- if( tilt*signTilt>coeffAffinePan )
- { tiltLimited = coeffAffinePan; }
- else { tiltLimited = tilt; }
- panLimited = pan;
+ const dynamicgraph::Vector &joint = jointSIN(timeSpec);
+ jointLimited = joint;
+
+ const double &pan = joint(panRank);
+ const double &tilt = joint(tiltRank);
+ double &panLimited = jointLimited(panRank);
+ double &tiltLimited = jointLimited(tiltRank);
+
+ if (fabs(pan) < 1e-3) // pan == 0
+ {
+ sotDEBUG(15) << "Pan = 0" << std::endl;
+ if (tilt * signTilt > coeffAffinePan) {
+ tiltLimited = coeffAffinePan;
+ } else {
+ tiltLimited = tilt;
}
- else if( pan>0 )
- {
- sotDEBUG(15) << "Pan > 0" << std::endl;
- if( signTilt*tilt > (pan*coeffLinearPan+coeffAffinePan) )
- {
- // Orthogonal projection .
-// if( (tilt-coeffAffinePan)*coeffLinearPan<-1*pan )
-// {
-// panLimited=0; tiltLimited=coeffAffinePan;
-// }
-// else
-// {
-// double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
-// double tmp2=pan+coeffLinearPan*tilt;
-
-// panLimited=(tmp2-coeffAffinePan*coeffLinearPan)*tmp;
-// tiltLimited=(coeffLinearPan*tmp2+coeffAffinePan)*tmp;
-// }
- tiltLimited = (pan*coeffLinearPan+coeffAffinePan);
- panLimited = pan;
- }
- else { tiltLimited = tilt; panLimited = pan; }
+ panLimited = pan;
+ } else if (pan > 0) {
+ sotDEBUG(15) << "Pan > 0" << std::endl;
+ if (signTilt * tilt > (pan * coeffLinearPan + coeffAffinePan)) {
+ // Orthogonal projection .
+ // if( (tilt-coeffAffinePan)*coeffLinearPan<-1*pan )
+ // {
+ // panLimited=0; tiltLimited=coeffAffinePan;
+ // }
+ // else
+ // {
+ // double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
+ // double tmp2=pan+coeffLinearPan*tilt;
+
+ // panLimited=(tmp2-coeffAffinePan*coeffLinearPan)*tmp;
+ // tiltLimited=(coeffLinearPan*tmp2+coeffAffinePan)*tmp;
+ // }
+ tiltLimited = (pan * coeffLinearPan + coeffAffinePan);
+ panLimited = pan;
+ } else {
+ tiltLimited = tilt;
+ panLimited = pan;
}
- 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 )
-// {
-// sotDEBUG(15) << "Proj on 0" << std::endl;
-// panLimited=0; tiltLimited=coeffAffinePan;
-// }
-// else
-// {
-// double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
-// double tmp2=pan-coeffLinearPan*tilt;
-
-// panLimited=(tmp2+coeffAffinePan*coeffLinearPan)*tmp;
-// tiltLimited=(-coeffLinearPan*tmp2+coeffAffinePan)*tmp;
-// }
- tiltLimited = (-pan*coeffLinearPan+coeffAffinePan);
- panLimited = pan;
- }
- else { tiltLimited = tilt; panLimited = pan; }
+ } 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 )
+ // {
+ // sotDEBUG(15) << "Proj on 0" << std::endl;
+ // panLimited=0; tiltLimited=coeffAffinePan;
+ // }
+ // else
+ // {
+ // double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
+ // double tmp2=pan-coeffLinearPan*tilt;
+
+ // panLimited=(tmp2+coeffAffinePan*coeffLinearPan)*tmp;
+ // tiltLimited=(-coeffLinearPan*tmp2+coeffAffinePan)*tmp;
+ // }
+ tiltLimited = (-pan * coeffLinearPan + coeffAffinePan);
+ panLimited = pan;
+ } else {
+ tiltLimited = tilt;
+ panLimited = pan;
}
-
-
+ }
sotDEBUGOUT(15);
return jointLimited;
@@ -142,8 +135,6 @@ computeJointLimitation( dynamicgraph::Vector& jointLimited,const int& timeSpec )
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
-void NeckLimitation::
-display( std::ostream& os ) const
-{
+void NeckLimitation::display(std::ostream &os) const {
os << "NeckLimitation " << getName() << "." << std::endl;
}
diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp
index 83661012..5d511fd1 100644
--- a/src/tools/parameter-server.cpp
+++ b/src/tools/parameter-server.cpp
@@ -14,317 +14,289 @@
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
#include <iostream>
-#include <sot/core/parameter-server.hh>
#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/all-commands.h>
-
-
-namespace dynamicgraph
-{
- namespace sot
- {
- namespace dynamicgraph = ::dynamicgraph;
- using namespace dynamicgraph;
- using namespace dynamicgraph::command;
- using namespace std;
+#include <sot/core/parameter-server.hh>
+namespace dynamicgraph {
+namespace sot {
+namespace dynamicgraph = ::dynamicgraph;
+using namespace dynamicgraph;
+using namespace dynamicgraph::command;
+using namespace std;
- //Size to be aligned "-------------------------------------------------------"
-#define PROFILE_PWM_DESIRED_COMPUTATION "Control manager "
-#define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period "
+// Size to be aligned "-------------------------------------------------------"
+#define PROFILE_PWM_DESIRED_COMPUTATION \
+ "Control manager "
+#define PROFILE_DYNAMIC_GRAPH_PERIOD \
+ "Control period "
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
- /// Define EntityClassName here rather than in the header file
- /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef ParameterServer EntityClassName;
-
- /* --- DG FACTORY ---------------------------------------------------- */
- 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)
- ,m_sleep_time(0.0)
- {
-
- //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
-
- /* Commands. */
- addCommand("init",
- makeCommandVoid3(*this, &ParameterServer::init,
- docCommandVoid3("Initialize the entity.",
- "Time period in seconds (double)",
- "URDF file path (string)",
- "Robot reference (string)")));
-
- addCommand("setNameToId",
- makeCommandVoid2(*this,&ParameterServer::setNameToId,
- docCommandVoid2("Set map for a name to an Id",
- "(string) joint name",
- "(double) joint id")));
-
- addCommand("setForceNameToForceId",
- makeCommandVoid2(*this,&ParameterServer::setForceNameToForceId,
- docCommandVoid2("Set map from a force sensor name to a force sensor Id",
- "(string) force sensor name",
- "(double) force sensor id")));
-
-
- addCommand("setJointLimitsFromId",
- makeCommandVoid3(*this,&ParameterServer::setJointLimitsFromId,
- docCommandVoid3("Set the joint limits for a given joint ID",
- "(double) joint id",
- "(double) lower limit",
- "(double) uppper limit")));
-
- addCommand("setForceLimitsFromId",
- makeCommandVoid3(*this,&ParameterServer::setForceLimitsFromId,
- docCommandVoid3("Set the force limits for a given force sensor ID",
- "(double) force sensor id",
- "(double) lower limit",
- "(double) uppper limit")));
-
- addCommand("setJointsUrdfToSot",
- makeCommandVoid1(*this, &ParameterServer::setJoints,
- docCommandVoid1("Map Joints From URDF to SoT.",
- "Vector of integer for mapping")));
-
- addCommand("setRightFootSoleXYZ",
- makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
- docCommandVoid1("Set the right foot sole 3d position.",
- "Vector of 3 doubles")));
- addCommand("setRightFootForceSensorXYZ",
- makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
- docCommandVoid1("Set the right foot sensor 3d position.",
- "Vector of 3 doubles")));
-
- addCommand("setFootFrameName",
- makeCommandVoid2(*this, &ParameterServer::setFootFrameName,
- docCommandVoid2("Set the Frame Name for the Foot Name.",
- "(string) Foot name",
- "(string) Frame name")));
- addCommand("setImuJointName",
- makeCommandVoid1(*this, &ParameterServer::setImuJointName,
- docCommandVoid1("Set the Joint Name wich IMU is attached to.",
- "(string) Joint name")));
- addCommand("displayRobotUtil",
- makeCommandVoid0(*this, &ParameterServer::displayRobotUtil,
- docCommandVoid0("Display the current robot util data set.")));
-
- }
-
- 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);
- m_dt = dt;
- m_emergency_stop_triggered = false;
- m_initSucceeded = true;
-
- std::string localName(robotRef);
- if (!isNameInRobotUtil(localName))
- {
- m_robot_util = createRobotUtil(localName);
- }
- else
- {
- m_robot_util = getRobotUtil(localName);
- }
-
- m_robot_util->m_urdf_filename = urdfFile;
-
- addCommand("getJointsUrdfToSot",
- makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
- docDirectSetter("Display map Joints From URDF to SoT.",
- "Vector of integer for mapping")));
-
- }
-
-
- /* ------------------------------------------------------------------- */
- /* --- SIGNALS ------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
-
-
-
- /* --- COMMANDS ---------------------------------------------------------- */
-
- void ParameterServer::setNameToId(const std::string &jointName,
- const double & jointId)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id before initialization!");
- return;
- }
- m_robot_util->set_name_to_id(jointName,static_cast<Index>(jointId));
- }
-
- void ParameterServer::setJointLimitsFromId( const double &jointId,
- const double &lq,
- const double &uq)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set joints limits from joint id before initialization!");
- return;
- }
-
- m_robot_util->set_joint_limits_for_id((Index)jointId,lq,uq);
- }
-
- void ParameterServer::setForceLimitsFromId( const double &jointId,
- const dynamicgraph::Vector &lq,
- const dynamicgraph::Vector &uq)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set force limits from force id before initialization!");
- return;
- }
-
- m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId,lq,uq);
- }
-
- 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!");
- return;
- }
-
- m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Index>(forceId));
- }
-
- void ParameterServer::setJoints(const dg::Vector & urdf_to_sot)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
- return;
- }
- m_robot_util->set_urdf_to_sot(urdf_to_sot);
- }
-
- void ParameterServer::setRightFootSoleXYZ( const dynamicgraph::Vector &xyz)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set right foot sole XYZ before initialization!");
- return;
- }
-
- m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
- }
-
- void ParameterServer::setRightFootForceSensorXYZ(const dynamicgraph::Vector &xyz)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set right foot force sensor XYZ before initialization!");
- return;
- }
-
- m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
- }
-
- void ParameterServer::setFootFrameName( const std::string &FootName,
- const std::string &FrameName)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
- return;
- }
- if (FootName=="Left")
- m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
- else if (FootName=="Right")
- m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
- else
- SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
- }
-
- void ParameterServer::setImuJointName(const std::string &JointName)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
- return;
- }
- m_robot_util->m_imu_joint_name = JointName;
- }
-
- void ParameterServer::displayRobotUtil()
- {
- m_robot_util->display(std::cout);
- }
-
- /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
-
- bool ParameterServer::convertJointNameToJointId(const std::string& name, unsigned int& id)
- {
- // Check if the joint name exists
- sot::Index jid = m_robot_util->get_id_from_name(name);
- if (jid<0)
- {
- SEND_MSG("The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
- std::stringstream ss;
- for(long unsigned int it=0; it< m_robot_util->m_nbJoints;it++)
- ss<< m_robot_util->get_name_from_id(it) <<", ";
- SEND_MSG("Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
- return false;
- }
- id = (unsigned int )jid;
- return true;
- }
-
- bool ParameterServer::isJointInRange(unsigned int id, double q)
- {
- const JointLimits & JL = m_robot_util->
- get_joint_limits_from_id((Index)id);
-
- double jl= JL.lower;
- if(q<jl)
- {
- SEND_MSG("Desired joint angle "+toString(q)+" is smaller than lower limit: "+toString(jl),MSG_TYPE_ERROR);
- return false;
- }
- double ju = JL.upper;
- if(q>ju)
- {
- SEND_MSG("Desired joint angle "+toString(q)+" is larger than upper limit: "+toString(ju),MSG_TYPE_ERROR);
- return false;
- }
- return true;
- }
-
-
- /* ------------------------------------------------------------------- */
- /* --- ENTITY -------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
-
- void ParameterServer::display(std::ostream& os) const
- {
- os << "ParameterServer "<<getName();
- }
- } // namespace sot
+/// Define EntityClassName here rather than in the header file
+/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+typedef ParameterServer EntityClassName;
+
+/* --- DG FACTORY ---------------------------------------------------- */
+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),
+ m_sleep_time(0.0) {
+
+ //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
+
+ /* Commands. */
+ addCommand("init",
+ makeCommandVoid3(*this, &ParameterServer::init,
+ docCommandVoid3("Initialize the entity.",
+ "Time period in seconds (double)",
+ "URDF file path (string)",
+ "Robot reference (string)")));
+
+ addCommand("setNameToId",
+ makeCommandVoid2(*this, &ParameterServer::setNameToId,
+ docCommandVoid2("Set map for a name to an Id",
+ "(string) joint name",
+ "(double) joint id")));
+
+ addCommand(
+ "setForceNameToForceId",
+ makeCommandVoid2(
+ *this, &ParameterServer::setForceNameToForceId,
+ docCommandVoid2(
+ "Set map from a force sensor name to a force sensor Id",
+ "(string) force sensor name", "(double) force sensor id")));
+
+ addCommand("setJointLimitsFromId",
+ makeCommandVoid3(
+ *this, &ParameterServer::setJointLimitsFromId,
+ docCommandVoid3("Set the joint limits for a given joint ID",
+ "(double) joint id", "(double) lower limit",
+ "(double) uppper limit")));
+
+ addCommand(
+ "setForceLimitsFromId",
+ makeCommandVoid3(
+ *this, &ParameterServer::setForceLimitsFromId,
+ docCommandVoid3("Set the force limits for a given force sensor ID",
+ "(double) force sensor id", "(double) lower limit",
+ "(double) uppper limit")));
+
+ addCommand(
+ "setJointsUrdfToSot",
+ makeCommandVoid1(*this, &ParameterServer::setJoints,
+ docCommandVoid1("Map Joints From URDF to SoT.",
+ "Vector of integer for mapping")));
+
+ addCommand(
+ "setRightFootSoleXYZ",
+ makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
+ docCommandVoid1("Set the right foot sole 3d position.",
+ "Vector of 3 doubles")));
+ addCommand(
+ "setRightFootForceSensorXYZ",
+ makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
+ docCommandVoid1("Set the right foot sensor 3d position.",
+ "Vector of 3 doubles")));
+
+ addCommand("setFootFrameName",
+ makeCommandVoid2(
+ *this, &ParameterServer::setFootFrameName,
+ docCommandVoid2("Set the Frame Name for the Foot Name.",
+ "(string) Foot name", "(string) Frame name")));
+ addCommand("setImuJointName",
+ makeCommandVoid1(
+ *this, &ParameterServer::setImuJointName,
+ docCommandVoid1("Set the Joint Name wich IMU is attached to.",
+ "(string) Joint name")));
+ addCommand("displayRobotUtil",
+ makeCommandVoid0(
+ *this, &ParameterServer::displayRobotUtil,
+ docCommandVoid0("Display the current robot util data set.")));
+}
+
+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);
+ m_dt = dt;
+ m_emergency_stop_triggered = false;
+ m_initSucceeded = true;
+
+ std::string localName(robotRef);
+ if (!isNameInRobotUtil(localName)) {
+ m_robot_util = createRobotUtil(localName);
+ } else {
+ m_robot_util = getRobotUtil(localName);
+ }
+
+ m_robot_util->m_urdf_filename = urdfFile;
+
+ addCommand(
+ "getJointsUrdfToSot",
+ makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
+ docDirectSetter("Display map Joints From URDF to SoT.",
+ "Vector of integer for mapping")));
+}
+
+/* ------------------------------------------------------------------- */
+/* --- SIGNALS ------------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+
+/* --- COMMANDS ---------------------------------------------------------- */
+
+void ParameterServer::setNameToId(const std::string &jointName,
+ const double &jointId) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG(
+ "Cannot set joint name from joint id before initialization!");
+ return;
+ }
+ m_robot_util->set_name_to_id(jointName, static_cast<Index>(jointId));
+}
+
+void ParameterServer::setJointLimitsFromId(const double &jointId,
+ const double &lq, const double &uq) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG(
+ "Cannot set joints limits from joint id before initialization!");
+ return;
+ }
+
+ m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq);
+}
+
+void ParameterServer::setForceLimitsFromId(const double &jointId,
+ const dynamicgraph::Vector &lq,
+ const dynamicgraph::Vector &uq) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG(
+ "Cannot set force limits from force id before initialization!");
+ return;
+ }
+
+ m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq);
+}
+
+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!");
+ return;
+ }
+
+ m_robot_util->m_force_util.set_name_to_force_id(forceName,
+ static_cast<Index>(forceId));
+}
+
+void ParameterServer::setJoints(const dg::Vector &urdf_to_sot) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
+ return;
+ }
+ m_robot_util->set_urdf_to_sot(urdf_to_sot);
+}
+
+void ParameterServer::setRightFootSoleXYZ(const dynamicgraph::Vector &xyz) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG(
+ "Cannot set right foot sole XYZ before initialization!");
+ return;
+ }
+
+ m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
+}
+
+void ParameterServer::setRightFootForceSensorXYZ(
+ const dynamicgraph::Vector &xyz) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG(
+ "Cannot set right foot force sensor XYZ before initialization!");
+ return;
+ }
+
+ m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
+}
+
+void ParameterServer::setFootFrameName(const std::string &FootName,
+ const std::string &FrameName) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
+ return;
+ }
+ if (FootName == "Left")
+ m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
+ else if (FootName == "Right")
+ m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
+ else
+ SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
+}
+
+void ParameterServer::setImuJointName(const std::string &JointName) {
+ if (!m_initSucceeded) {
+ SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
+ return;
+ }
+ m_robot_util->m_imu_joint_name = JointName;
+}
+
+void ParameterServer::displayRobotUtil() { m_robot_util->display(std::cout); }
+
+/* --- PROTECTED MEMBER METHODS
+ * ---------------------------------------------------------- */
+
+bool ParameterServer::convertJointNameToJointId(const std::string &name,
+ unsigned int &id) {
+ // Check if the joint name exists
+ sot::Index jid = m_robot_util->get_id_from_name(name);
+ if (jid < 0) {
+ SEND_MSG("The specified joint name does not exist: " + name,
+ MSG_TYPE_ERROR);
+ std::stringstream ss;
+ for (long unsigned int it = 0; it < m_robot_util->m_nbJoints; it++)
+ ss << m_robot_util->get_name_from_id(it) << ", ";
+ SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
+ return false;
+ }
+ id = (unsigned int)jid;
+ return true;
+}
+
+bool ParameterServer::isJointInRange(unsigned int id, double q) {
+ const JointLimits &JL = m_robot_util->get_joint_limits_from_id((Index)id);
+
+ double jl = JL.lower;
+ if (q < jl) {
+ SEND_MSG("Desired joint angle " + toString(q) +
+ " is smaller than lower limit: " + toString(jl),
+ MSG_TYPE_ERROR);
+ return false;
+ }
+ double ju = JL.upper;
+ if (q > ju) {
+ SEND_MSG("Desired joint angle " + toString(q) +
+ " is larger than upper limit: " + toString(ju),
+ MSG_TYPE_ERROR);
+ return false;
+ }
+ return true;
+}
+
+/* ------------------------------------------------------------------- */
+/* --- ENTITY -------------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+
+void ParameterServer::display(std::ostream &os) const {
+ os << "ParameterServer " << getName();
+}
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/tools/periodic-call-entity.cpp b/src/tools/periodic-call-entity.cpp
index 40d09cac..dd03de13 100644
--- a/src/tools/periodic-call-entity.cpp
+++ b/src/tools/periodic-call-entity.cpp
@@ -12,61 +12,46 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
-#include <sot/core/periodic-call-entity.hh>
#include <dynamic-graph/pool.h>
#include <sot/core/debug.hh>
#include <sot/core/factory.hh>
+#include <sot/core/periodic-call-entity.hh>
using namespace std;
using namespace dynamicgraph::sot;
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PeriodicCallEntity,"PeriodicCallEntity");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PeriodicCallEntity, "PeriodicCallEntity");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+PeriodicCallEntity::PeriodicCallEntity(const string &fName)
+ : Entity(fName), PeriodicCall(), triger("Tracer(" + fName + ")::triger"),
+ trigerOnce("Tracer(" + fName + ")::trigerOnce") {
+ signalRegistration(triger << trigerOnce);
-
-PeriodicCallEntity::
-PeriodicCallEntity( const string& fName )
- : Entity( fName )
- ,PeriodicCall()
- ,triger( "Tracer("+fName+")::triger" )
- ,trigerOnce( "Tracer("+fName+")::trigerOnce" )
-{
- signalRegistration( triger << trigerOnce );
-
- triger.setFunction( boost::bind(&PeriodicCallEntity::trigerCall,this,_1,_2) );
- trigerOnce.setFunction( boost::bind(&PeriodicCallEntity::trigerOnceCall,
- this,_1,_2) );
-
+ triger.setFunction(
+ boost::bind(&PeriodicCallEntity::trigerCall, this, _1, _2));
+ trigerOnce.setFunction(
+ boost::bind(&PeriodicCallEntity::trigerOnceCall, this, _1, _2));
}
-int& PeriodicCallEntity::
-trigerCall( int& dummy,const int & time )
-{
+int &PeriodicCallEntity::trigerCall(int &dummy, const int &time) {
run(time);
return dummy;
}
-int& PeriodicCallEntity::
-trigerOnceCall( int& dummy,const int & time )
-{
- run( time );
+int &PeriodicCallEntity::trigerOnceCall(int &dummy, const int &time) {
+ run(time);
clear();
return dummy;
}
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-void PeriodicCallEntity::
-display( std::ostream& os ) const
-{
- os << "PeriodicCallEntity <"<<name<<"> ";
- PeriodicCall::display( os );
+void PeriodicCallEntity::display(std::ostream &os) const {
+ os << "PeriodicCallEntity <" << name << "> ";
+ PeriodicCall::display(os);
}
diff --git a/src/tools/periodic-call.cpp b/src/tools/periodic-call.cpp
index c6ebe422..c7b44067 100644
--- a/src/tools/periodic-call.cpp
+++ b/src/tools/periodic-call.cpp
@@ -12,13 +12,13 @@
/* --------------------------------------------------------------------- */
/* --- SOT --- */
-#include <sot/core/periodic-call.hh>
-#include <dynamic-graph/pool.h>
-#include <sot/core/debug.hh>
-#include <sot/core/exception-tools.hh>
#include <algorithm>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/exception-factory.h>
+#include <dynamic-graph/pool.h>
+#include <sot/core/debug.hh>
+#include <sot/core/exception-tools.hh>
+#include <sot/core/periodic-call.hh>
using namespace std;
using namespace dynamicgraph;
@@ -28,99 +28,74 @@ using namespace dynamicgraph::sot;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-
-PeriodicCall::
-PeriodicCall( void )
- : signalMap()
- ,innerTime( 0 )
-{
-
-}
+PeriodicCall::PeriodicCall(void) : signalMap(), innerTime(0) {}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void PeriodicCall::
-addSignal( const std::string &name, SignalBase<int>& sig )
-{
- signalMap[ name ] = SignalToCall(&sig);
- return ;
+void PeriodicCall::addSignal(const std::string &name, SignalBase<int> &sig) {
+ signalMap[name] = SignalToCall(&sig);
+ return;
}
-void PeriodicCall::
-addSignal( const std::string& sigpath )
-{
- istringstream sigISS( sigpath );
- SignalBase<int>& signal = ::dynamicgraph::PoolStorage::getInstance()->getSignal( sigISS );
- addSignal( sigpath,signal );
- return ;
+void PeriodicCall::addSignal(const std::string &sigpath) {
+ istringstream sigISS(sigpath);
+ SignalBase<int> &signal =
+ ::dynamicgraph::PoolStorage::getInstance()->getSignal(sigISS);
+ addSignal(sigpath, signal);
+ return;
}
-void PeriodicCall::
-addDownsampledSignal( const std::string &name, SignalBase<int>& sig, const unsigned int& downsamplingFactor )
-{
- signalMap[ name ] = SignalToCall(&sig,downsamplingFactor);
- return ;
+void PeriodicCall::addDownsampledSignal(
+ const std::string &name, SignalBase<int> &sig,
+ const unsigned int &downsamplingFactor) {
+ signalMap[name] = SignalToCall(&sig, downsamplingFactor);
+ return;
}
-void PeriodicCall::
-addDownsampledSignal( const std::string& sigpath, const unsigned int& downsamplingFactor )
-{
- istringstream sigISS( sigpath );
- SignalBase<int>& signal = ::dynamicgraph::PoolStorage::getInstance()->getSignal( sigISS );
- addDownsampledSignal( sigpath,signal,downsamplingFactor );
- return ;
+void PeriodicCall::addDownsampledSignal(
+ const std::string &sigpath, const unsigned int &downsamplingFactor) {
+ istringstream sigISS(sigpath);
+ SignalBase<int> &signal =
+ ::dynamicgraph::PoolStorage::getInstance()->getSignal(sigISS);
+ addDownsampledSignal(sigpath, signal, downsamplingFactor);
+ return;
}
-void PeriodicCall::
-rmSignal( const std::string &name )
-{
- signalMap.erase( name );
- return ;
+void PeriodicCall::rmSignal(const std::string &name) {
+ signalMap.erase(name);
+ return;
}
-
-
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-void PeriodicCall::
-runSignals( const int& t )
-{
- for( SignalMapType::iterator iter = signalMap.begin();
- signalMap.end()!=iter; ++iter )
- {
- if(t%iter->second.downsamplingFactor == 0)
- (*iter).second.signal->recompute( t );
- }
- return ;
+void PeriodicCall::runSignals(const int &t) {
+ for (SignalMapType::iterator iter = signalMap.begin();
+ signalMap.end() != iter; ++iter) {
+ if (t % iter->second.downsamplingFactor == 0)
+ (*iter).second.signal->recompute(t);
+ }
+ return;
}
-void PeriodicCall::
-run( const int & t )
-{
- runSignals( t );
- return ;
+void PeriodicCall::run(const int &t) {
+ runSignals(t);
+ return;
}
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
+void PeriodicCall::display(std::ostream &os) const {
+ os << " (t=" << innerTime << ")" << endl;
-void PeriodicCall::
-display( std::ostream& os ) const
-{
- os <<" (t=" << innerTime << ")" <<endl;
-
- os<<" -> SIGNALS:"<<endl;
- for( SignalMapType::const_iterator iter = signalMap.begin();
- signalMap.end()!=iter; ++iter )
- {
- os << " - " << (*iter).first << endl;
- }
-
+ os << " -> SIGNALS:" << endl;
+ for (SignalMapType::const_iterator iter = signalMap.begin();
+ signalMap.end() != iter; ++iter) {
+ os << " - " << (*iter).first << endl;
+ }
}
/*
@@ -137,60 +112,61 @@ static std::string readLineStr( istringstream& args )
return res;
}
*/
-#define ADD_COMMAND( name,def ) \
-if (commandMap.count(prefix+name) != 0) { \
- DG_THROW ExceptionFactory(ExceptionFactory::OBJECT_CONFLICT, \
- "Command " + prefix+name + \
- " already registered in Entity."); \
- } \
-commandMap.insert( std::make_pair( prefix+name,def ) )
-
-
-void PeriodicCall::addSpecificCommands(Entity& ent,
- Entity::CommandMap_t& commandMap,
- const std::string& prefix )
-{
+#define ADD_COMMAND(name, def) \
+ if (commandMap.count(prefix + name) != 0) { \
+ DG_THROW ExceptionFactory(ExceptionFactory::OBJECT_CONFLICT, \
+ "Command " + prefix + name + \
+ " already registered in Entity."); \
+ } \
+ commandMap.insert(std::make_pair(prefix + name, def))
+
+void PeriodicCall::addSpecificCommands(Entity &ent,
+ Entity::CommandMap_t &commandMap,
+ const std::string &prefix) {
using namespace dynamicgraph::command;
/* Explicit typage to help the compiler. */
- boost::function< void( const std::string& ) >
- addSignal = boost::bind( &PeriodicCall::addSignal, this,_1 ),
- rmSignal = boost::bind( &PeriodicCall::rmSignal, this,_1 );
- boost::function< void( const std::string&, const unsigned int& ) >
- addDownsampledSignal = boost::bind( &PeriodicCall::addDownsampledSignal, this,_1,_2);
- boost::function< void( void ) >
- clear = boost::bind( &PeriodicCall::clear, this );
- boost::function< void( std::ostream& ) >
- disp = boost::bind( &PeriodicCall::display, this,_1 );
-
- ADD_COMMAND("addSignal",
- makeCommandVoid1(ent,addSignal,
- docCommandVoid1("Add the signal to the refresh list",
- "string (sig name)")));
- ADD_COMMAND("addDownsampledSignal",
- makeCommandVoid2(ent,addDownsampledSignal,
- docCommandVoid2("Add the signal to the refresh list",
- "string (sig name)",
- "unsigned int (downsampling factor, 1 means every time, 2 means every other time, etc...")));
- ADD_COMMAND("rmSignal",
- makeCommandVoid1(ent,rmSignal,
- docCommandVoid1("Remove the signal to the refresh list",
- "string (sig name)")));
- ADD_COMMAND("clear",
- makeCommandVoid0(ent,clear,
- docCommandVoid0("Clear all signals and commands from the refresh list.")));
-
- ADD_COMMAND("disp",
- makeCommandVerbose(ent,disp,
- docCommandVerbose("Print the list of to-refresh signals and commands.")));
-
+ boost::function<void(const std::string &)>
+ addSignal = boost::bind(&PeriodicCall::addSignal, this, _1),
+ rmSignal = boost::bind(&PeriodicCall::rmSignal, this, _1);
+ boost::function<void(const std::string &, const unsigned int &)>
+ addDownsampledSignal =
+ boost::bind(&PeriodicCall::addDownsampledSignal, this, _1, _2);
+ boost::function<void(void)> clear = boost::bind(&PeriodicCall::clear, this);
+ boost::function<void(std::ostream &)> disp =
+ boost::bind(&PeriodicCall::display, this, _1);
+
+ ADD_COMMAND(
+ "addSignal",
+ makeCommandVoid1(ent, addSignal,
+ docCommandVoid1("Add the signal to the refresh list",
+ "string (sig name)")));
+ ADD_COMMAND("addDownsampledSignal",
+ makeCommandVoid2(
+ ent, addDownsampledSignal,
+ docCommandVoid2(
+ "Add the signal to the refresh list", "string (sig name)",
+ "unsigned int (downsampling factor, 1 means every time, "
+ "2 means every other time, etc...")));
+ ADD_COMMAND(
+ "rmSignal",
+ makeCommandVoid1(ent, rmSignal,
+ docCommandVoid1("Remove the signal to the refresh list",
+ "string (sig name)")));
+ ADD_COMMAND(
+ "clear",
+ makeCommandVoid0(
+ ent, clear,
+ docCommandVoid0(
+ "Clear all signals and commands from the refresh list.")));
+
+ ADD_COMMAND("disp",
+ makeCommandVerbose(
+ ent, disp,
+ docCommandVerbose(
+ "Print the list of to-refresh signals and commands.")));
}
-
-
-
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/src/tools/robot-simu.cpp b/src/tools/robot-simu.cpp
index 64672ea0..a456953d 100644
--- a/src/tools/robot-simu.cpp
+++ b/src/tools/robot-simu.cpp
@@ -6,42 +6,34 @@
*
*/
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/all-commands.h>
#include "sot/core/robot-simu.hh"
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
namespace dynamicgraph {
- namespace sot {
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu,"RobotSimu");
-
- 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";
- 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";
- addCommand("setTimeStep",
- makeDirectSetter (*this, &this->timestep_,
- docstring));
+namespace sot {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu, "RobotSimu");
+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";
+ addCommand("increment", command::makeCommandVoid1(
+ (Device &)*this, &Device::increment, docstring));
- }
- } // namespace sot
+ /* Set Time step. */
+ 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
diff --git a/src/tools/robot-utils-py.cpp b/src/tools/robot-utils-py.cpp
index be68e8a0..8897f5c9 100644
--- a/src/tools/robot-utils-py.cpp
+++ b/src/tools/robot-utils-py.cpp
@@ -5,68 +5,62 @@
* See license file.
*/
-#include <sot/core/robot-utils.hh>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/map_indexing_suite.hpp>
+#include <sot/core/robot-utils.hh>
using namespace boost::python;
using namespace dynamicgraph::sot;
+BOOST_PYTHON_MODULE(robot_utils_sot_py) {
+ class_<JointLimits>("JointLimits", init<double, double>())
+ .def_readwrite("upper", &JointLimits::upper)
+ .def_readwrite("lower", &JointLimits::lower);
-BOOST_PYTHON_MODULE(robot_utils_sot_py)
-{
- class_<JointLimits>
- ("JointLimits",init<double,double>())
- .def_readwrite("upper",&JointLimits::upper)
- .def_readwrite("lower",&JointLimits::lower)
- ;
-
- class_<ForceLimits>("ForceLimits",init<const Eigen::VectorXd &,const Eigen::VectorXd &>())
- .def("display",&ForceLimits::display)
- .def_readwrite("upper",&ForceLimits::upper)
- .def_readwrite("lower",&ForceLimits::lower)
- ;
+ class_<ForceLimits>("ForceLimits",
+ init<const Eigen::VectorXd &, const Eigen::VectorXd &>())
+ .def("display", &ForceLimits::display)
+ .def_readwrite("upper", &ForceLimits::upper)
+ .def_readwrite("lower", &ForceLimits::lower);
class_<ForceUtil>("ForceUtil")
- .def("set_name_to_force_id", &ForceUtil::set_name_to_force_id)
- .def("set_force_id_to_limits", &ForceUtil::set_force_id_to_limits)
- .def("create_force_id_to_name_map",&ForceUtil::create_force_id_to_name_map)
- .def("get_id_from_name", &ForceUtil::get_id_from_name)
- .def("get_name_from_id", &ForceUtil::cp_get_name_from_id)
- .def("get_limits_from_id", &ForceUtil::cp_get_limits_from_id)
- .def("get_force_id_left_hand", &ForceUtil::get_force_id_left_hand)
- .def("set_force_id_left_hand", &ForceUtil::set_force_id_left_hand)
- .def("get_force_id_right_hand", &ForceUtil::get_force_id_right_hand)
- .def("set_force_id_right_hand", &ForceUtil::set_force_id_right_hand)
- .def("get_force_id_left_foot", &ForceUtil::get_force_id_left_foot)
- .def("set_force_id_left_foot", &ForceUtil::set_force_id_left_foot)
- .def("get_force_id_right_foot", &ForceUtil::get_force_id_right_foot)
- .def("set_force_id_right_foot", &ForceUtil::set_force_id_right_foot)
- .def("display", &ForceUtil::display)
- ;
+ .def("set_name_to_force_id", &ForceUtil::set_name_to_force_id)
+ .def("set_force_id_to_limits", &ForceUtil::set_force_id_to_limits)
+ .def("create_force_id_to_name_map",
+ &ForceUtil::create_force_id_to_name_map)
+ .def("get_id_from_name", &ForceUtil::get_id_from_name)
+ .def("get_name_from_id", &ForceUtil::cp_get_name_from_id)
+ .def("get_limits_from_id", &ForceUtil::cp_get_limits_from_id)
+ .def("get_force_id_left_hand", &ForceUtil::get_force_id_left_hand)
+ .def("set_force_id_left_hand", &ForceUtil::set_force_id_left_hand)
+ .def("get_force_id_right_hand", &ForceUtil::get_force_id_right_hand)
+ .def("set_force_id_right_hand", &ForceUtil::set_force_id_right_hand)
+ .def("get_force_id_left_foot", &ForceUtil::get_force_id_left_foot)
+ .def("set_force_id_left_foot", &ForceUtil::set_force_id_left_foot)
+ .def("get_force_id_right_foot", &ForceUtil::get_force_id_right_foot)
+ .def("set_force_id_right_foot", &ForceUtil::set_force_id_right_foot)
+ .def("display", &ForceUtil::display);
class_<RobotUtil>("RobotUtil")
- .def_readwrite("m_force_util",&RobotUtil::m_force_util)
- .def_readwrite("m_foot_util",&RobotUtil::m_foot_util)
- .def_readwrite("m_urdf_to_sot",&RobotUtil::m_urdf_to_sot)
- .def_readonly("m_nbJoints",&RobotUtil::m_nbJoints)
- .def_readwrite("m_name_to_id",&RobotUtil::m_name_to_id)
- .def_readwrite("m_id_to_name",&RobotUtil::m_id_to_name)
- .def("set_joint_limits_for_id",&RobotUtil::set_joint_limits_for_id)
- .def("get_joint_limits_from_id",&RobotUtil::cp_get_joint_limits_from_id)
- //.def("set_joint_limits_for_id",&RobotUtil::set_joint_limits_for_id)
- //.def("set_name_to_id", &RobotUtil::set_name_to_id)
- //.def("create_id_to_name_map",&RobotUtil::create_id_to_name_map)
- //.def("get_id_from_name",&RobotUtil::get_id_from_name)
- ;
+ .def_readwrite("m_force_util", &RobotUtil::m_force_util)
+ .def_readwrite("m_foot_util", &RobotUtil::m_foot_util)
+ .def_readwrite("m_urdf_to_sot", &RobotUtil::m_urdf_to_sot)
+ .def_readonly("m_nbJoints", &RobotUtil::m_nbJoints)
+ .def_readwrite("m_name_to_id", &RobotUtil::m_name_to_id)
+ .def_readwrite("m_id_to_name", &RobotUtil::m_id_to_name)
+ .def("set_joint_limits_for_id", &RobotUtil::set_joint_limits_for_id)
+ .def("get_joint_limits_from_id", &RobotUtil::cp_get_joint_limits_from_id)
+ //.def("set_joint_limits_for_id",&RobotUtil::set_joint_limits_for_id)
+ //.def("set_name_to_id", &RobotUtil::set_name_to_id)
+ //.def("create_id_to_name_map",&RobotUtil::create_id_to_name_map)
+ //.def("get_id_from_name",&RobotUtil::get_id_from_name)
+ ;
+ class_<std::map<Index, ForceLimits>>("IndexForceLimits")
+ .def(map_indexing_suite<std::map<Index, ForceLimits>>());
- class_<std::map<Index,ForceLimits> >("IndexForceLimits")
- .def(map_indexing_suite<std::map<Index,ForceLimits> > ());
+ class_<std::map<std::string, Index>>("stringIndex")
+ .def(map_indexing_suite<std::map<std::string, Index>>());
- class_<std::map<std::string,Index> >("stringIndex")
- .def(map_indexing_suite<std::map<std::string,Index> > ());
-
- class_<std::map<Index,std::string> >("Indexstring")
- .def(map_indexing_suite<std::map<Index,std::string> > ());
-
+ class_<std::map<Index, std::string>>("Indexstring")
+ .def(map_indexing_suite<std::map<Index, std::string>>());
}
diff --git a/src/tools/robot-utils.cpp b/src/tools/robot-utils.cpp
index 924fc8d7..b59bff55 100644
--- a/src/tools/robot-utils.cpp
+++ b/src/tools/robot-utils.cpp
@@ -5,552 +5,455 @@
*
*/
-#include <iostream>
-#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
-#include <sot/core/robot-utils.hh>
-#include <sot/core/debug.hh>
+#include <boost/shared_ptr.hpp>
#include <dynamic-graph/factory.h>
+#include <iostream>
+#include <sot/core/debug.hh>
+#include <sot/core/robot-utils.hh>
-namespace dynamicgraph
-{
- namespace sot
- {
- namespace dg = ::dynamicgraph;
- using namespace dg;
- using namespace dg::command;
-
- RobotUtil VoidRobotUtil;
- ForceLimits VoidForceLimits;
- JointLimits VoidJointLimits;
- Index VoidIndex(-1);
-
- RobotUtilShrPtr RefVoidRobotUtil()
- {
- return boost::make_shared<RobotUtil>(VoidRobotUtil);
- }
-
- void ForceLimits::display(std::ostream &os) const
- {
- os << "Lower limits:" << std::endl;
- os << lower << std::endl;
- os << "Upper Limits:" << std::endl;
- os << upper << std::endl;
- }
-
- /******************** FootUtil ***************************/
-
- void FootUtil::display(std::ostream &os) const
- {
- os << "Right Foot Sole XYZ " << std::endl;
- os << m_Right_Foot_Sole_XYZ << std::endl;
- os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
- os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
- }
-
- /******************** HandUtil ***************************/
-
- void HandUtil::display(std::ostream &os) const
- {
- os << "Left Hand Frame Name:" << m_Left_Hand_Frame_Name << std::endl;
- os << "Right Hand Frame Name:" << m_Right_Hand_Frame_Name << std::endl;
- }
-
- /******************** ForceUtil ***************************/
-
- void ForceUtil::set_name_to_force_id(const std::string &name,
- const Index &force_id)
- {
- m_name_to_force_id[name] = (Index)force_id;
- create_force_id_to_name_map();
- if (name == "rf")
- set_force_id_right_foot(m_name_to_force_id[name]);
- else if (name == "lf")
- set_force_id_left_foot(m_name_to_force_id[name]);
- else if (name == "lh")
- set_force_id_left_hand(m_name_to_force_id[name]);
- else if (name == "rh")
- set_force_id_right_hand(m_name_to_force_id[name]);
- }
-
- void ForceUtil::set_force_id_to_limits(const Index &force_id,
- const dg::Vector &lf,
- const dg::Vector &uf)
- {
- m_force_id_to_limits[(Index)force_id].lower = lf;
- m_force_id_to_limits[(Index)force_id].upper = uf;
- }
-
- 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;
- return -1;
- }
-
- std::string force_default_rtn("Force name not found");
- 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;
- return force_default_rtn;
- }
-
- std::string ForceUtil::cp_get_name_from_id(Index idx)
- {
- const std::string &default_rtn = get_name_from_id(idx);
- return default_rtn;
- }
- void ForceUtil::create_force_id_to_name_map()
- {
- std::map<std::string, Index>::const_iterator it;
- for (it = m_name_to_force_id.begin();
- it != m_name_to_force_id.end(); it++)
- m_force_id_to_name[it->second] = it->first;
- }
-
- 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 iter->second;
- }
-
- 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 iter->second;
- }
-
- void ForceUtil::display(std::ostream &os) const
- {
- os << "Force Id to limits " << std::endl;
- for (std::map<Index, ForceLimits>::const_iterator
- it = m_force_id_to_limits.begin();
- it != m_force_id_to_limits.end();
- ++it)
- {
- it->second.display(os);
- }
-
- os << "Name to force id:" << std::endl;
- for (std::map<std::string, Index>::const_iterator
- it = m_name_to_force_id.begin();
- it != m_name_to_force_id.end();
- ++it)
- {
- os << "(" << it->first << "," << it->second << ") ";
- }
- os << std::endl;
-
- os << "Force id to Name:" << std::endl;
- for (std::map<Index, std::string>::const_iterator
- it = m_force_id_to_name.begin();
- it != m_force_id_to_name.end();
- ++it)
- {
- os << "(" << it->first << "," << it->second << ") ";
- }
- os << std::endl;
-
- os << "Index for force sensors:" << std::endl;
- os << "Left Hand (" << m_Force_Id_Left_Hand << ") ,"
- << "Right Hand (" << m_Force_Id_Right_Hand << ") ,"
- << "Left Foot (" << m_Force_Id_Left_Foot << ") ,"
- << "Right Foot (" << m_Force_Id_Right_Foot << ") "
- << std::endl;
- }
-
- /**************** FromURDFToSot *************************/
- RobotUtil::RobotUtil()
- {
- }
-
- void RobotUtil::
- set_joint_limits_for_id(const Index &idx,
- const double &lq,
- const double &uq)
- {
- m_limits_map[(Index)idx] = JointLimits(lq, uq);
- }
-
- 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;
- return iter->second;
- }
- JointLimits RobotUtil::
- cp_get_joint_limits_from_id(Index id)
- {
- const JointLimits &rtn = get_joint_limits_from_id(id);
- return rtn;
- }
-
- void RobotUtil::
- set_name_to_id(const std::string &jointName,
- const Index &jointId)
- {
- m_name_to_id[jointName] = (Index)jointId;
- create_id_to_name_map();
- }
-
- void RobotUtil::
- create_id_to_name_map()
- {
- std::map<std::string, Index>::const_iterator it;
- for (it = m_name_to_id.begin(); it != m_name_to_id.end(); it++)
- m_id_to_name[it->second] = it->first;
- }
-
- 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;
- 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;
- return iter->second;
- }
-
- void RobotUtil::
- set_urdf_to_sot(const std::vector<Index> &urdf_to_sot)
- {
- m_nbJoints = urdf_to_sot.size();
- m_urdf_to_sot.resize(urdf_to_sot.size());
- m_dgv_urdf_to_sot.resize(urdf_to_sot.size());
- for (std::size_t idx = 0;
- idx < urdf_to_sot.size(); idx++)
- {
- m_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
- m_dgv_urdf_to_sot[(Index)idx] =
- static_cast<double>(urdf_to_sot[(Index)idx]);
- }
- }
-
- void RobotUtil::
- set_urdf_to_sot(const dg::Vector &urdf_to_sot)
- {
- m_nbJoints = urdf_to_sot.size();
- m_urdf_to_sot.resize(urdf_to_sot.size());
- for (unsigned int idx = 0; idx < urdf_to_sot.size(); idx++)
- {
- m_urdf_to_sot[idx] = (unsigned int)urdf_to_sot[idx];
- }
- m_dgv_urdf_to_sot = urdf_to_sot;
- }
-
- bool RobotUtil::
- joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
- {
- if (m_nbJoints == 0)
- {
- SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
- return false;
- }
- assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
- assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
-
- for (unsigned int idx = 0; idx < m_nbJoints; idx++)
- q_sot[m_urdf_to_sot[idx]] = q_urdf[idx];
- return true;
- }
-
- bool RobotUtil::
- joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
- {
- assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
- assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
-
- if (m_nbJoints == 0)
- {
- SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
- return false;
- }
-
- for (unsigned int idx = 0; idx < m_nbJoints; idx++)
- q_urdf[idx] = q_sot[m_urdf_to_sot[idx]];
- return true;
- }
-
- bool RobotUtil::
- velocity_urdf_to_sot(ConstRefVector q_urdf, ConstRefVector v_urdf,
- RefVector v_sot)
- {
- assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
- assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
- assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
-
- if (m_nbJoints == 0)
- {
- SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
- return false;
- }
- const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
- Eigen::Matrix3d oRb = q.toRotationMatrix();
- v_sot.head<3>() = oRb * v_urdf.head<3>();
- v_sot.segment<3>(3) = oRb * v_urdf.segment<3>(3);
- // v_sot.head<6>() = v_urdf.head<6>();
- joints_urdf_to_sot(v_urdf.tail(m_nbJoints),
- v_sot.tail(m_nbJoints));
- return true;
- }
-
- bool RobotUtil::
- velocity_sot_to_urdf(ConstRefVector q_urdf, ConstRefVector v_sot,
- RefVector v_urdf)
- {
- assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
- assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
- assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
-
- if (m_nbJoints == 0)
- {
- SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
- return false;
- }
- // compute rotation from world to base frame
- const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
- Eigen::Matrix3d oRb = q.toRotationMatrix();
- v_urdf.head<3>() = oRb.transpose() * v_sot.head<3>();
- v_urdf.segment<3>(3) = oRb.transpose() * v_sot.segment<3>(3);
- // v_urdf.head<6>() = v_sot.head<6>();
- joints_sot_to_urdf(v_sot.tail(m_nbJoints),
- v_urdf.tail(m_nbJoints));
- return true;
- }
-
- bool RobotUtil::
- base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
- {
- assert(q_urdf.size() == 7);
- assert(q_sot.size() == 6);
-
- // ********* Quat to RPY *********
- const double W = q_urdf[6];
- const double X = q_urdf[3];
- const double Y = q_urdf[4];
- const double Z = q_urdf[5];
- const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
- return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
- }
-
- bool RobotUtil::
- base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
- {
- assert(q_urdf.size() == 7);
- assert(q_sot.size() == 6);
- // ********* RPY to Quat *********
- const double r = q_sot[3];
- const double p = q_sot[4];
- const double y = q_sot[5];
- const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
- const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
- 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[1] = q_sot[1];
- q_urdf[2] = q_sot[2];
- q_urdf[3] = quat.x();
- q_urdf[4] = quat.y();
- q_urdf[5] = quat.z();
- q_urdf[6] = quat.w();
-
- return true;
- }
-
- bool RobotUtil::
- config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
- {
- assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
- assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
-
- base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
- joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
-
- return true;
- }
-
- bool RobotUtil::
- config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
- {
- assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
- assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
- base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
- joints_sot_to_urdf(q_sot.tail(m_nbJoints),
- q_urdf.tail(m_nbJoints));
- return true;
- }
- void RobotUtil::
- display(std::ostream &os) const
- {
- m_force_util.display(os);
- m_foot_util.display(os);
- m_hand_util.display(os);
- os << "Nb of joints: " << m_nbJoints << std::endl;
- os << "Urdf file name: " << m_urdf_filename << std::endl;
-
- // Display m_urdf_to_sot
- os << "Map from urdf index to the Sot Index " << std::endl;
- for (unsigned int i = 0; i < m_urdf_to_sot.size(); i++)
- os << "(" << i << " : " << m_urdf_to_sot[i] << ") ";
- os << std::endl;
-
- os << "Joint name to joint id:" << std::endl;
- for (std::map<std::string, Index>::const_iterator
- it = m_name_to_id.begin();
- it != m_name_to_id.end();
- ++it)
- {
- os << "(" << it->first << "," << it->second << ") ";
- }
- os << std::endl;
-
- os << "Joint id to joint Name:" << std::endl;
- for (std::map<Index, std::string>::const_iterator
- it = m_id_to_name.begin();
- it != m_id_to_name.end();
- ++it)
- {
- os << "(" << it->first << "," << it->second << ") ";
- }
- os << std::endl;
- }
- void RobotUtil::
- sendMsg(const std::string &msg,
- MsgType t,
- const char *file,
- int line)
- {
- logger_.sendMsg("[RobotUtil]" + msg, t, file, line);
- }
- bool base_se3_to_sot(ConstRefVector pos,
- ConstRefMatrix R,
- RefVector q_sot)
- {
- assert(q_sot.size() == 6);
- assert(pos.size() == 3);
- assert(R.rows() == 3);
- assert(R.cols() == 3);
- // ********* Quat to RPY *********
- double r, p, y, m;
- m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
- p = atan2(-R(2, 0), m);
- if (fabs(fabs(p) - M_PI / 2) < 0.001)
- {
- r = 0.0;
- y = -atan2(R(0, 1), R(1, 1));
- }
- else
- {
- y = atan2(R(1, 0), R(0, 0));
- r = atan2(R(2, 1), R(2, 2));
- }
- // *********************************
- q_sot[0] = pos[0];
- q_sot[1] = pos[1];
- q_sot[2] = pos[2];
- q_sot[3] = r;
- q_sot[4] = p;
- q_sot[5] = y;
- return true;
- }
-
- bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot)
- {
- assert(q_urdf.size() == 7);
- assert(q_sot.size() == 6);
- // ********* Quat to RPY *********
- const double W = q_urdf[6];
- const double X = q_urdf[3];
- const double Y = q_urdf[4];
- const double Z = q_urdf[5];
- const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
- return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
- }
-
- bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf)
- {
- assert(q_urdf.size() == 7);
- assert(q_sot.size() == 6);
- // ********* RPY to Quat *********
- const double r = q_sot[3];
- const double p = q_sot[4];
- const double y = q_sot[5];
- const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
- const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
- 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[1] = q_sot[1];
- q_urdf[2] = q_sot[2];
- q_urdf[3] = quat.x();
- q_urdf[4] = quat.y();
- q_urdf[5] = quat.z();
- q_urdf[6] = quat.w();
-
- return true;
- }
-
- std::map<std::string, RobotUtilShrPtr> sgl_map_name_to_robot_util;
-
- 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;
- 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;
- 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())
- {
- sgl_map_name_to_robot_util[robotName] =
- boost::shared_ptr<RobotUtil>(new RobotUtil);
- it = sgl_map_name_to_robot_util.find(robotName);
- return it->second;
- }
- std::cout << "Another robot is already in the map for " << robotName
- << std::endl;
- return RefVoidRobotUtil();
- }
- } // namespace sot
+namespace dynamicgraph {
+namespace sot {
+namespace dg = ::dynamicgraph;
+using namespace dg;
+using namespace dg::command;
+
+RobotUtil VoidRobotUtil;
+ForceLimits VoidForceLimits;
+JointLimits VoidJointLimits;
+Index VoidIndex(-1);
+
+RobotUtilShrPtr RefVoidRobotUtil() {
+ return boost::make_shared<RobotUtil>(VoidRobotUtil);
+}
+
+void ForceLimits::display(std::ostream &os) const {
+ os << "Lower limits:" << std::endl;
+ os << lower << std::endl;
+ os << "Upper Limits:" << std::endl;
+ os << upper << std::endl;
+}
+
+/******************** FootUtil ***************************/
+
+void FootUtil::display(std::ostream &os) const {
+ os << "Right Foot Sole XYZ " << std::endl;
+ os << m_Right_Foot_Sole_XYZ << std::endl;
+ os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
+ os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
+}
+
+/******************** HandUtil ***************************/
+
+void HandUtil::display(std::ostream &os) const {
+ os << "Left Hand Frame Name:" << m_Left_Hand_Frame_Name << std::endl;
+ os << "Right Hand Frame Name:" << m_Right_Hand_Frame_Name << std::endl;
+}
+
+/******************** ForceUtil ***************************/
+
+void ForceUtil::set_name_to_force_id(const std::string &name,
+ const Index &force_id) {
+ m_name_to_force_id[name] = (Index)force_id;
+ create_force_id_to_name_map();
+ if (name == "rf")
+ set_force_id_right_foot(m_name_to_force_id[name]);
+ else if (name == "lf")
+ set_force_id_left_foot(m_name_to_force_id[name]);
+ else if (name == "lh")
+ set_force_id_left_hand(m_name_to_force_id[name]);
+ else if (name == "rh")
+ set_force_id_right_hand(m_name_to_force_id[name]);
+}
+
+void ForceUtil::set_force_id_to_limits(const Index &force_id,
+ const dg::Vector &lf,
+ const dg::Vector &uf) {
+ m_force_id_to_limits[(Index)force_id].lower = lf;
+ m_force_id_to_limits[(Index)force_id].upper = uf;
+}
+
+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;
+ return -1;
+}
+
+std::string force_default_rtn("Force name not found");
+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;
+ return force_default_rtn;
+}
+
+std::string ForceUtil::cp_get_name_from_id(Index idx) {
+ const std::string &default_rtn = get_name_from_id(idx);
+ return default_rtn;
+}
+void ForceUtil::create_force_id_to_name_map() {
+ std::map<std::string, Index>::const_iterator it;
+ for (it = m_name_to_force_id.begin(); it != m_name_to_force_id.end(); it++)
+ m_force_id_to_name[it->second] = it->first;
+}
+
+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 iter->second;
+}
+
+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 iter->second;
+}
+
+void ForceUtil::display(std::ostream &os) const {
+ os << "Force Id to limits " << std::endl;
+ for (std::map<Index, ForceLimits>::const_iterator it =
+ m_force_id_to_limits.begin();
+ it != m_force_id_to_limits.end(); ++it) {
+ it->second.display(os);
+ }
+
+ os << "Name to force id:" << std::endl;
+ for (std::map<std::string, Index>::const_iterator it =
+ m_name_to_force_id.begin();
+ it != m_name_to_force_id.end(); ++it) {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+
+ os << "Force id to Name:" << std::endl;
+ for (std::map<Index, std::string>::const_iterator it =
+ m_force_id_to_name.begin();
+ it != m_force_id_to_name.end(); ++it) {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+
+ os << "Index for force sensors:" << std::endl;
+ os << "Left Hand (" << m_Force_Id_Left_Hand << ") ,"
+ << "Right Hand (" << m_Force_Id_Right_Hand << ") ,"
+ << "Left Foot (" << m_Force_Id_Left_Foot << ") ,"
+ << "Right Foot (" << m_Force_Id_Right_Foot << ") " << std::endl;
+}
+
+/**************** FromURDFToSot *************************/
+RobotUtil::RobotUtil() {}
+
+void RobotUtil::set_joint_limits_for_id(const Index &idx, const double &lq,
+ const double &uq) {
+ m_limits_map[(Index)idx] = JointLimits(lq, uq);
+}
+
+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;
+ return iter->second;
+}
+JointLimits RobotUtil::cp_get_joint_limits_from_id(Index id) {
+ const JointLimits &rtn = get_joint_limits_from_id(id);
+ return rtn;
+}
+
+void RobotUtil::set_name_to_id(const std::string &jointName,
+ const Index &jointId) {
+ m_name_to_id[jointName] = (Index)jointId;
+ create_id_to_name_map();
+}
+
+void RobotUtil::create_id_to_name_map() {
+ std::map<std::string, Index>::const_iterator it;
+ for (it = m_name_to_id.begin(); it != m_name_to_id.end(); it++)
+ m_id_to_name[it->second] = it->first;
+}
+
+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;
+ 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;
+ return iter->second;
+}
+
+void RobotUtil::set_urdf_to_sot(const std::vector<Index> &urdf_to_sot) {
+ m_nbJoints = urdf_to_sot.size();
+ m_urdf_to_sot.resize(urdf_to_sot.size());
+ m_dgv_urdf_to_sot.resize(urdf_to_sot.size());
+ for (std::size_t idx = 0; idx < urdf_to_sot.size(); idx++) {
+ m_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
+ m_dgv_urdf_to_sot[(Index)idx] =
+ static_cast<double>(urdf_to_sot[(Index)idx]);
+ }
+}
+
+void RobotUtil::set_urdf_to_sot(const dg::Vector &urdf_to_sot) {
+ m_nbJoints = urdf_to_sot.size();
+ m_urdf_to_sot.resize(urdf_to_sot.size());
+ for (unsigned int idx = 0; idx < urdf_to_sot.size(); idx++) {
+ m_urdf_to_sot[idx] = (unsigned int)urdf_to_sot[idx];
+ }
+ m_dgv_urdf_to_sot = urdf_to_sot;
+}
+
+bool RobotUtil::joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+ if (m_nbJoints == 0) {
+ SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+ assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+ assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+
+ for (unsigned int idx = 0; idx < m_nbJoints; idx++)
+ q_sot[m_urdf_to_sot[idx]] = q_urdf[idx];
+ return true;
+}
+
+bool RobotUtil::joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+ assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+ assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+
+ if (m_nbJoints == 0) {
+ SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+
+ for (unsigned int idx = 0; idx < m_nbJoints; idx++)
+ q_urdf[idx] = q_sot[m_urdf_to_sot[idx]];
+ return true;
+}
+
+bool RobotUtil::velocity_urdf_to_sot(ConstRefVector q_urdf,
+ ConstRefVector v_urdf, RefVector v_sot) {
+ assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+ assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+ assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+
+ if (m_nbJoints == 0) {
+ SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+ const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
+ Eigen::Matrix3d oRb = q.toRotationMatrix();
+ v_sot.head<3>() = oRb * v_urdf.head<3>();
+ v_sot.segment<3>(3) = oRb * v_urdf.segment<3>(3);
+ // v_sot.head<6>() = v_urdf.head<6>();
+ joints_urdf_to_sot(v_urdf.tail(m_nbJoints), v_sot.tail(m_nbJoints));
+ return true;
+}
+
+bool RobotUtil::velocity_sot_to_urdf(ConstRefVector q_urdf,
+ ConstRefVector v_sot, RefVector v_urdf) {
+ assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+ assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+ assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+
+ if (m_nbJoints == 0) {
+ SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
+ return false;
+ }
+ // compute rotation from world to base frame
+ const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
+ Eigen::Matrix3d oRb = q.toRotationMatrix();
+ v_urdf.head<3>() = oRb.transpose() * v_sot.head<3>();
+ v_urdf.segment<3>(3) = oRb.transpose() * v_sot.segment<3>(3);
+ // v_urdf.head<6>() = v_sot.head<6>();
+ joints_sot_to_urdf(v_sot.tail(m_nbJoints), v_urdf.tail(m_nbJoints));
+ return true;
+}
+
+bool RobotUtil::base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+ assert(q_urdf.size() == 7);
+ assert(q_sot.size() == 6);
+
+ // ********* Quat to RPY *********
+ const double W = q_urdf[6];
+ const double X = q_urdf[3];
+ const double Y = q_urdf[4];
+ const double Z = q_urdf[5];
+ const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
+ return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
+}
+
+bool RobotUtil::base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+ assert(q_urdf.size() == 7);
+ assert(q_sot.size() == 6);
+ // ********* RPY to Quat *********
+ const double r = q_sot[3];
+ const double p = q_sot[4];
+ const double y = q_sot[5];
+ const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
+ const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
+ 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[1] = q_sot[1];
+ q_urdf[2] = q_sot[2];
+ q_urdf[3] = quat.x();
+ q_urdf[4] = quat.y();
+ q_urdf[5] = quat.z();
+ q_urdf[6] = quat.w();
+
+ return true;
+}
+
+bool RobotUtil::config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+ assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+ assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+
+ base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
+ joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
+
+ return true;
+}
+
+bool RobotUtil::config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+ assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+ assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+ base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
+ joints_sot_to_urdf(q_sot.tail(m_nbJoints), q_urdf.tail(m_nbJoints));
+ return true;
+}
+void RobotUtil::display(std::ostream &os) const {
+ m_force_util.display(os);
+ m_foot_util.display(os);
+ m_hand_util.display(os);
+ os << "Nb of joints: " << m_nbJoints << std::endl;
+ os << "Urdf file name: " << m_urdf_filename << std::endl;
+
+ // Display m_urdf_to_sot
+ os << "Map from urdf index to the Sot Index " << std::endl;
+ for (unsigned int i = 0; i < m_urdf_to_sot.size(); i++)
+ os << "(" << i << " : " << m_urdf_to_sot[i] << ") ";
+ os << std::endl;
+
+ os << "Joint name to joint id:" << std::endl;
+ for (std::map<std::string, Index>::const_iterator it = m_name_to_id.begin();
+ it != m_name_to_id.end(); ++it) {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+
+ os << "Joint id to joint Name:" << std::endl;
+ for (std::map<Index, std::string>::const_iterator it = m_id_to_name.begin();
+ it != m_id_to_name.end(); ++it) {
+ os << "(" << it->first << "," << it->second << ") ";
+ }
+ os << std::endl;
+}
+void RobotUtil::sendMsg(const std::string &msg, MsgType t, const char *file,
+ int line) {
+ logger_.sendMsg("[RobotUtil]" + msg, t, file, line);
+}
+bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot) {
+ assert(q_sot.size() == 6);
+ assert(pos.size() == 3);
+ assert(R.rows() == 3);
+ assert(R.cols() == 3);
+ // ********* Quat to RPY *********
+ double r, p, y, m;
+ m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
+ p = atan2(-R(2, 0), m);
+ if (fabs(fabs(p) - M_PI / 2) < 0.001) {
+ r = 0.0;
+ y = -atan2(R(0, 1), R(1, 1));
+ } else {
+ y = atan2(R(1, 0), R(0, 0));
+ r = atan2(R(2, 1), R(2, 2));
+ }
+ // *********************************
+ q_sot[0] = pos[0];
+ q_sot[1] = pos[1];
+ q_sot[2] = pos[2];
+ q_sot[3] = r;
+ q_sot[4] = p;
+ q_sot[5] = y;
+ return true;
+}
+
+bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+ assert(q_urdf.size() == 7);
+ assert(q_sot.size() == 6);
+ // ********* Quat to RPY *********
+ const double W = q_urdf[6];
+ const double X = q_urdf[3];
+ const double Y = q_urdf[4];
+ const double Z = q_urdf[5];
+ const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
+ return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
+}
+
+bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+ assert(q_urdf.size() == 7);
+ assert(q_sot.size() == 6);
+ // ********* RPY to Quat *********
+ const double r = q_sot[3];
+ const double p = q_sot[4];
+ const double y = q_sot[5];
+ const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
+ const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
+ 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[1] = q_sot[1];
+ q_urdf[2] = q_sot[2];
+ q_urdf[3] = quat.x();
+ q_urdf[4] = quat.y();
+ q_urdf[5] = quat.z();
+ q_urdf[6] = quat.w();
+
+ return true;
+}
+
+std::map<std::string, RobotUtilShrPtr> sgl_map_name_to_robot_util;
+
+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;
+ 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;
+ 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()) {
+ sgl_map_name_to_robot_util[robotName] =
+ boost::shared_ptr<RobotUtil>(new RobotUtil);
+ it = sgl_map_name_to_robot_util.find(robotName);
+ return it->second;
+ }
+ std::cout << "Another robot is already in the map for " << robotName
+ << std::endl;
+ return RefVoidRobotUtil();
+}
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/tools/seq-play.cpp b/src/tools/seq-play.cpp
index d320ca23..2bf78184 100644
--- a/src/tools/seq-play.cpp
+++ b/src/tools/seq-play.cpp
@@ -12,8 +12,8 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/seq-play.hh>
#include <sot/core/debug.hh>
+#include <sot/core/seq-play.hh>
using namespace std;
#include <fstream>
@@ -23,53 +23,47 @@ using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SeqPlay,"SeqPlay");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SeqPlay, "SeqPlay");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-SeqPlay::
-SeqPlay( const std::string& n )
- :Entity(n)
- ,stateList()
- ,currPos(stateList.begin())
- ,currRank(0)
- ,init(false)
- ,time(0)
- ,refresherSINTERN( "SeqPlay("+n+")::intern(dummy)::refresher" )
- ,positionSOUT( boost::bind(&SeqPlay::getNextPosition,this,_1,_2),
- refresherSINTERN,
- "SeqPlay("+n+")::output(vector)::position" )
-{
- signalRegistration( positionSOUT );
- refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
+SeqPlay::SeqPlay(const std::string &n)
+ : Entity(n), stateList(), currPos(stateList.begin()), currRank(0),
+ init(false), time(0),
+ refresherSINTERN("SeqPlay(" + n + ")::intern(dummy)::refresher"),
+ positionSOUT(boost::bind(&SeqPlay::getNextPosition, this, _1, _2),
+ refresherSINTERN,
+ "SeqPlay(" + n + ")::output(vector)::position") {
+ signalRegistration(positionSOUT);
+ refresherSINTERN.setDependencyType(TimeDependency<int>::ALWAYS_READY);
}
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
/* --- COMPUTE ----------------------------------------------------------- */
-dynamicgraph::Vector& SeqPlay::
-getNextPosition( dynamicgraph::Vector& pos, const int& /*time*/ )
-{
+dynamicgraph::Vector &SeqPlay::getNextPosition(dynamicgraph::Vector &pos,
+ const int & /*time*/) {
sotDEBUGIN(15);
- if( !init )
- {
- if( stateList.empty() ) return pos;
- currPos=stateList.begin(); init=true; currRank = 0;
- }
-
-
- {
- const dynamicgraph::Vector& posCur = *currPos;
- pos=posCur;
-
- currPos++;
- if( currPos==stateList.end() ) currPos--;
- else currRank++;
- }
+ if (!init) {
+ if (stateList.empty())
+ return pos;
+ currPos = stateList.begin();
+ init = true;
+ currRank = 0;
+ }
+
+ {
+ const dynamicgraph::Vector &posCur = *currPos;
+ pos = posCur;
+
+ currPos++;
+ if (currPos == stateList.end())
+ currPos--;
+ else
+ currRank++;
+ }
sotDEBUGOUT(15);
return pos;
@@ -78,48 +72,48 @@ getNextPosition( dynamicgraph::Vector& pos, const int& /*time*/ )
/* --- LIST -------------------------------------------------------------- */
/* --- LIST -------------------------------------------------------------- */
/* --- LIST -------------------------------------------------------------- */
-void SeqPlay::
-loadFile( const std::string& filename )
-{
+void SeqPlay::loadFile(const std::string &filename) {
sotDEBUGIN(15);
- sotDEBUG( 25 ) << " Load " << filename << endl;
+ sotDEBUG(25) << " Load " << filename << endl;
std::ifstream file(filename.c_str());
const unsigned int SIZE = 1024;
char buffer[SIZE];
- dynamicgraph::Vector res(1); unsigned int ressize = 1;
+ dynamicgraph::Vector res(1);
+ unsigned int ressize = 1;
double time;
- while( file.good() )
- {
- file.getline( buffer,SIZE );
- if( file.gcount()<5 ) break;
+ while (file.good()) {
+ file.getline(buffer, SIZE);
+ if (file.gcount() < 5)
+ break;
- sotDEBUG(25) << buffer<<endl;
- std::istringstream iss( buffer );
+ sotDEBUG(25) << buffer << endl;
+ std::istringstream iss(buffer);
- iss>>time; unsigned int i;
+ iss >> time;
+ unsigned int i;
- for( i=0;iss.good();++i )
- {
- if( i==ressize ) { ressize*=2; res.resize(ressize,false); }
- iss>>res(i); sotDEBUG(35) <<i<< ": " << res(i)<<endl;
- }
- ressize=i-1; res.resize(ressize,false);
- stateList.push_back( res );
- sotDEBUG(15) << time << ": " << res << endl;
+ for (i = 0; iss.good(); ++i) {
+ if (i == ressize) {
+ ressize *= 2;
+ res.resize(ressize, false);
+ }
+ iss >> res(i);
+ sotDEBUG(35) << i << ": " << res(i) << endl;
}
+ ressize = i - 1;
+ res.resize(ressize, false);
+ stateList.push_back(res);
+ sotDEBUG(15) << time << ": " << res << endl;
+ }
sotDEBUGOUT(15);
}
-
-
-
/* --- DISPLAY ------------------------------------------------------------ */
/* --- DISPLAY ------------------------------------------------------------ */
/* --- DISPLAY ------------------------------------------------------------ */
-void SeqPlay::display ( std::ostream& os ) const
-{os <<name<<endl; }
+void SeqPlay::display(std::ostream &os) const { os << name << endl; }
diff --git a/src/tools/sequencer.cpp b/src/tools/sequencer.cpp
index a343675c..2cd18134 100644
--- a/src/tools/sequencer.cpp
+++ b/src/tools/sequencer.cpp
@@ -7,41 +7,32 @@
*
*/
-#include <sot/core/sequencer.hh>
+#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>
#include <sot/core/sot.hh>
-#include <dynamic-graph/pool.h>
-#include <dynamic-graph/factory.h>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sequencer,"Sequencer");
-
-Sequencer::
-Sequencer( const std::string & name )
- :Entity(name)
- ,timeInit(-1)
- ,playMode(false)
- ,outputStreamPtr(NULL)
- ,noOutput(false)
- ,triggerSOUT( boost::bind(&Sequencer::trigger,this,_1,_2),
- sotNOSIGNAL,
- "Sequencer("+name+")::output(dummy)::trigger" )
-{
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sequencer, "Sequencer");
+
+Sequencer::Sequencer(const std::string &name)
+ : Entity(name), timeInit(-1), playMode(false), outputStreamPtr(NULL),
+ noOutput(false),
+ triggerSOUT(boost::bind(&Sequencer::trigger, this, _1, _2), sotNOSIGNAL,
+ "Sequencer(" + name + ")::output(dummy)::trigger") {
sotDEBUGIN(5);
- signalRegistration( triggerSOUT );
- triggerSOUT.setNeedUpdateFromAllChildren( true );
+ signalRegistration(triggerSOUT);
+ triggerSOUT.setNeedUpdateFromAllChildren(true);
sotDEBUGOUT(5);
}
-
-Sequencer::
-~Sequencer( void )
-{
+Sequencer::~Sequencer(void) {
sotDEBUGIN(5);
sotDEBUGOUT(5);
@@ -52,221 +43,198 @@ Sequencer::
/* --- SPECIFIC EVENT ------------------------------------------------------- */
/* --- SPECIFIC EVENT ------------------------------------------------------- */
-class sotEventTaskBased
- : public Sequencer::sotEventAbstract
-{
+class sotEventTaskBased : public Sequencer::sotEventAbstract {
protected:
- TaskAbstract * taskPtr;
+ TaskAbstract *taskPtr;
const std::string defaultTaskName;
+
public:
- sotEventTaskBased( const std::string name = "",TaskAbstract* task = NULL )
- :sotEventAbstract( name )
- ,taskPtr( task )
- ,defaultTaskName("NULL")
- {}
+ sotEventTaskBased(const std::string name = "", TaskAbstract *task = NULL)
+ : sotEventAbstract(name), taskPtr(task), defaultTaskName("NULL") {}
- void init( std::istringstream& cmdArgs )
- {
+ void init(std::istringstream &cmdArgs) {
cmdArgs >> std::ws;
- if( cmdArgs.good() )
- {
- std::string taskname; cmdArgs >> taskname;
- sotDEBUG(15) << "Add task " << taskname << std::endl;
- taskPtr
- = dynamic_cast< TaskAbstract* >
- (&dg::PoolStorage::getInstance()->getEntity(taskname));
- }
+ if (cmdArgs.good()) {
+ std::string taskname;
+ cmdArgs >> taskname;
+ sotDEBUG(15) << "Add task " << taskname << std::endl;
+ taskPtr = dynamic_cast<TaskAbstract *>(
+ &dg::PoolStorage::getInstance()->getEntity(taskname));
+ }
+ }
+ virtual void display(std::ostream &os) const {
+ if (taskPtr)
+ os << taskPtr->getName();
+ else
+ os << "NULL";
+ }
+ virtual const std::string &getName() const {
+ if (taskPtr)
+ return taskPtr->getName();
+ else
+ return defaultTaskName;
}
- virtual void display( std::ostream& os ) const
- { if( taskPtr ) os << taskPtr->getName(); else os << "NULL"; }
- virtual const std::string& getName() const
- { if( taskPtr ) return taskPtr->getName(); else return defaultTaskName; }
-
};
-class sotEventAddATask
- : public sotEventTaskBased
-{
+class sotEventAddATask : public sotEventTaskBased {
public:
- sotEventAddATask( const std::string name = "",TaskAbstract* task=NULL )
- :sotEventTaskBased( name,task )
- {
+ sotEventAddATask(const std::string name = "", TaskAbstract *task = NULL)
+ : sotEventTaskBased(name, task) {
eventType = EVENT_ADD;
}
- void operator()( Sot* sotptr )
- {
+ void operator()(Sot *sotptr) {
sotDEBUGIN(15);
- sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "." << std::endl;
- if( (NULL!=sotptr)&&(NULL!=taskPtr) ) sotptr->push(*taskPtr );
+ sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
+ << std::endl;
+ if ((NULL != sotptr) && (NULL != taskPtr))
+ sotptr->push(*taskPtr);
sotDEBUGOUT(15);
}
- virtual void display( std::ostream& os ) const
- {os << "Add<"; sotEventTaskBased::display(os); os<<">"; }
-
+ virtual void display(std::ostream &os) const {
+ os << "Add<";
+ sotEventTaskBased::display(os);
+ os << ">";
+ }
};
-
-class sotEventRemoveATask
- : public sotEventTaskBased
-{
+class sotEventRemoveATask : public sotEventTaskBased {
public:
- sotEventRemoveATask( const std::string name = "",TaskAbstract* task=NULL )
- :sotEventTaskBased( name,task )
- {
+ sotEventRemoveATask(const std::string name = "", TaskAbstract *task = NULL)
+ : sotEventTaskBased(name, task) {
eventType = EVENT_RM;
}
- void operator()( Sot* sotptr )
- {
+ void operator()(Sot *sotptr) {
sotDEBUGIN(15);
- sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "." << std::endl;
- if( (NULL!=sotptr)&&(NULL!=taskPtr) ) sotptr->remove(*taskPtr );
+ sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
+ << std::endl;
+ if ((NULL != sotptr) && (NULL != taskPtr))
+ sotptr->remove(*taskPtr);
sotDEBUGOUT(15);
}
- virtual void display( std::ostream& os ) const
- { os << "Remove<"; sotEventTaskBased::display(os); os<<">"; }
-
+ virtual void display(std::ostream &os) const {
+ os << "Remove<";
+ sotEventTaskBased::display(os);
+ os << ">";
+ }
};
-
-class sotEventCmd
- : public Sequencer::sotEventAbstract
-{
+class sotEventCmd : public Sequencer::sotEventAbstract {
protected:
std::string cmd;
public:
- sotEventCmd( const std::string cmdLine = "" )
- :sotEventAbstract( cmdLine+"<cmd>" )
- ,cmd( cmdLine )
- {
+ sotEventCmd(const std::string cmdLine = "")
+ : sotEventAbstract(cmdLine + "<cmd>"), cmd(cmdLine) {
eventType = EVENT_CMD;
- sotDEBUGINOUT(15);
+ sotDEBUGINOUT(15);
}
- void init( std::istringstream& args )
- {
+ void init(std::istringstream &args) {
sotDEBUGIN(15);
- std::stringbuf* pbuf=args.rdbuf();
+ std::stringbuf *pbuf = args.rdbuf();
const unsigned int size = (unsigned int)(pbuf->in_avail());
- char * buffer = new char [size+1];
- pbuf->sgetn( buffer,size );
+ char *buffer = new char[size + 1];
+ pbuf->sgetn(buffer, size);
- buffer[size]='\0';
+ buffer[size] = '\0';
cmd = buffer;
sotDEBUGOUT(15);
- delete [] buffer;
+ delete[] buffer;
}
- const std::string & getEventCmd() const
- { return cmd; }
- virtual void display( std::ostream& os ) const
- { os << "Run: " << cmd; }
- virtual void operator() ( Sot* /*sotPtr*/ )
- {
- std::ostringstream onull; onull.clear( std::ios::failbit );
- std::istringstream iss( cmd );
- std::string cmdName; iss >> cmdName;
+ const std::string &getEventCmd() const { return cmd; }
+ virtual void display(std::ostream &os) const { os << "Run: " << cmd; }
+ virtual void operator()(Sot * /*sotPtr*/) {
+ std::ostringstream onull;
+ onull.clear(std::ios::failbit);
+ std::istringstream iss(cmd);
+ std::string cmdName;
+ iss >> cmdName;
// Florent: remove reference to g_shell
- //g_shell.cmd( cmdName,iss,onull );
- } ;
+ // g_shell.cmd( cmdName,iss,onull );
+ };
};
-
/* --- TASK MANIP ----------------------------------------------------------- */
/* --- TASK MANIP ----------------------------------------------------------- */
/* --- TASK MANIP ----------------------------------------------------------- */
-void Sequencer::
-addTask( sotEventAbstract* task,const unsigned int timeSpec )
-{
- TaskMap::iterator listKey = taskMap.find( timeSpec );
- if( taskMap.end()==listKey )
- {
- sotDEBUG(15) << "New element at " << timeSpec << std::endl;
- taskMap[timeSpec].push_back( task );
- }
- else
- {
- TaskList& tl = listKey->second;
- tl.push_back( task );
+void Sequencer::addTask(sotEventAbstract *task, const unsigned int timeSpec) {
+ TaskMap::iterator listKey = taskMap.find(timeSpec);
+ if (taskMap.end() == listKey) {
+ sotDEBUG(15) << "New element at " << timeSpec << std::endl;
+ taskMap[timeSpec].push_back(task);
+ } else {
+ TaskList &tl = listKey->second;
+ tl.push_back(task);
}
}
-//rmTask
-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
- {
- TaskList& tl = listKey->second;
- for( TaskList::iterator itL = tl.begin(); itL != tl.end(); ++itL)
- {
- if ((*itL)->getEventType() == eventType && (*itL)->getName() == name)
- {
- tl.remove(*itL);
- break;
- }
- }
-
- //remove the list if empty
- if (tl.empty())
- taskMap.erase(listKey);
- }
+// rmTask
+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
+ {
+ TaskList &tl = listKey->second;
+ for (TaskList::iterator itL = tl.begin(); itL != tl.end(); ++itL) {
+ if ((*itL)->getEventType() == eventType && (*itL)->getName() == name) {
+ tl.remove(*itL);
+ break;
+ }
+ }
+
+ // remove the list if empty
+ if (tl.empty())
+ taskMap.erase(listKey);
+ }
}
-//clearAll
-void Sequencer::
-clearAll( )
-{
+// clearAll
+void Sequencer::clearAll() {
TaskMap::iterator itM;
- for(itM = taskMap.begin(); itM != taskMap.end(); ++itM )
- {
- TaskList::iterator itL;
- TaskList& currentMap = itM->second;
- for (itL=currentMap.begin(); itL!=currentMap.end(); ++itL)
- delete (*itL);
- itM->second.clear();
+ for (itM = taskMap.begin(); itM != taskMap.end(); ++itM) {
+ TaskList::iterator itL;
+ TaskList ¤tMap = itM->second;
+ for (itL = currentMap.begin(); itL != currentMap.end(); ++itL)
+ delete (*itL);
+ itM->second.clear();
}
- //remove all the lists
+ // remove all the lists
taskMap.clear();
}
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
-int& Sequencer::
-trigger( int& dummy,const int& timeSpec )
-{
+int &Sequencer::trigger(int &dummy, const int &timeSpec) {
sotDEBUGIN(15);
- if(! playMode ) return dummy;
- if( -1==timeInit ) timeInit = timeSpec;
-
- sotDEBUG(15) << "Ref time: " << (timeSpec-timeInit) << std::endl;
- TaskMap::iterator listKey = taskMap.find( timeSpec-timeInit );
- if( taskMap.end()!=listKey )
- {
- sotDEBUG(1) << "Time: "<< (timeSpec-timeInit) << ": we've got a task to do!"
- << std::endl;
- TaskList & tl = listKey->second;
- for( TaskList::iterator iter=tl.begin();iter!=tl.end();++iter )
- {
- if( *iter )
- {
- (*iter)->operator() (sotPtr);
- if( NULL!=outputStreamPtr )
- {
- (*outputStreamPtr) << "At time t=" << timeSpec << ": ";
- (*iter)->display(*outputStreamPtr);
- (*outputStreamPtr) << std::endl;
- }
- }
- }
+ if (!playMode)
+ return dummy;
+ if (-1 == timeInit)
+ timeInit = timeSpec;
+
+ sotDEBUG(15) << "Ref time: " << (timeSpec - timeInit) << std::endl;
+ TaskMap::iterator listKey = taskMap.find(timeSpec - timeInit);
+ if (taskMap.end() != listKey) {
+ sotDEBUG(1) << "Time: " << (timeSpec - timeInit)
+ << ": we've got a task to do!" << std::endl;
+ TaskList &tl = listKey->second;
+ for (TaskList::iterator iter = tl.begin(); iter != tl.end(); ++iter) {
+ if (*iter) {
+ (*iter)->operator()(sotPtr);
+ if (NULL != outputStreamPtr) {
+ (*outputStreamPtr) << "At time t=" << timeSpec << ": ";
+ (*iter)->display(*outputStreamPtr);
+ (*outputStreamPtr) << std::endl;
+ }
+ }
}
+ }
sotDEBUGOUT(15);
return dummy;
@@ -276,24 +244,21 @@ trigger( int& dummy,const int& timeSpec )
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
-void Sequencer::
-display( std::ostream& os ) const
-{
- if (noOutput) return;
+void Sequencer::display(std::ostream &os) const {
+ if (noOutput)
+ return;
os << "Sequencer " << getName() << "(t0=" << timeInit
- << ",mode=" << ( (playMode)?"play":"pause" ) << "): " << std::endl;
- for( TaskMap::const_iterator iterMap = taskMap.begin();
- iterMap!=taskMap.end();iterMap++ )
- {
- os << " - t=" << (iterMap->first) << ":\t";
- const TaskList & tl = iterMap->second;
- for( TaskList::const_iterator iterList = tl.begin();
- iterList!=tl.end();iterList++ )
- {
- (*iterList)->display(os); os << " ";
- }
- os << std::endl;
+ << ",mode=" << ((playMode) ? "play" : "pause") << "): " << std::endl;
+ for (TaskMap::const_iterator iterMap = taskMap.begin();
+ iterMap != taskMap.end(); iterMap++) {
+ os << " - t=" << (iterMap->first) << ":\t";
+ const TaskList &tl = iterMap->second;
+ for (TaskList::const_iterator iterList = tl.begin(); iterList != tl.end();
+ iterList++) {
+ (*iterList)->display(os);
+ os << " ";
}
-
+ os << std::endl;
+ }
}
diff --git a/src/tools/smooth-reach.cpp b/src/tools/smooth-reach.cpp
index 34ac71ed..c19d3662 100644
--- a/src/tools/smooth-reach.cpp
+++ b/src/tools/smooth-reach.cpp
@@ -7,129 +7,113 @@
*
*/
-#include <sot/core/smooth-reach.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
+#include <sot/core/debug.hh>
+#include <sot/core/smooth-reach.hh>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SmoothReach, "SmoothReach");
-SmoothReach::SmoothReach(const std::string & name)
- : Entity(name)
+SmoothReach::SmoothReach(const std::string &name)
+ : Entity(name)
- ,start(0u), goal(0u)
- ,startTime(-1), lengthTime(-1)
- ,isStarted(false), isParam(true)
+ ,
+ start(0u), goal(0u), startTime(-1), lengthTime(-1), isStarted(false),
+ isParam(true)
- ,smoothMode(2), smoothParam(1.2)
-
- , startSIN(NULL, "SmoothReach("+name+")::input(vector)::start")
- ,goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
- sotNOSIGNAL,
- "SmoothReach("+name+")::output(vector)::goal")
+ ,
+ smoothMode(2), smoothParam(1.2)
+ ,
+ startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
+ goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
+ sotNOSIGNAL, "SmoothReach(" + name + ")::output(vector)::goal")
{
sotDEBUGIN(5);
- signalRegistration( startSIN << goalSOUT );
+ signalRegistration(startSIN << goalSOUT);
initCommands();
goalSOUT.setNeedUpdateFromAllChildren(true);
sotDEBUGOUT(5);
}
-void SmoothReach::
-initCommands( void )
-{
+void SmoothReach::initCommands(void) {
using namespace command;
- addCommand( "set",
- makeCommandVoid2( *this,&SmoothReach::set,
- docCommandVoid2( "Set the curve.",
- "vector (goal)", "int (duration)")));
- addCommand( "param",
- makeCommandVoid2( *this,&SmoothReach::setSmoothing,
- docCommandVoid2( "Set the parameter.",
- "int (mode)","double (beta)")));
+ addCommand("set",
+ makeCommandVoid2(*this, &SmoothReach::set,
+ docCommandVoid2("Set the curve.", "vector (goal)",
+ "int (duration)")));
+ addCommand("param",
+ makeCommandVoid2(*this, &SmoothReach::setSmoothing,
+ docCommandVoid2("Set the parameter.",
+ "int (mode)", "double (beta)")));
}
-
-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;
- }
- }
+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;
+ }
+ }
return 0;
}
-void SmoothReach::
-setSmoothing( const int & mode, const double & param )
-{ smoothMode= mode; smoothParam = param; }
+void SmoothReach::setSmoothing(const int &mode, const double ¶m) {
+ smoothMode = mode;
+ smoothParam = param;
+}
-dynamicgraph::Vector& SmoothReach::
-goalSOUT_function( dynamicgraph::Vector & res, const int& time)
-{
- if( isParam )
- {
- start = startSIN(time);
- startTime = time;
-
- assert( start.size() == goal.size() );
- isParam = false; isStarted = true;
- }
-
- if( isStarted )
- {
- double x = double(time-startTime)/lengthTime;
- if( x>1 ) x=1;
- double x1 = smoothFunction(x);
- double x0 = 1-x1;
- res = start*x0 + goal*x1;
- }
+dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &res,
+ const int &time) {
+ if (isParam) {
+ start = startSIN(time);
+ startTime = time;
+
+ assert(start.size() == goal.size());
+ isParam = false;
+ isStarted = true;
+ }
+
+ if (isStarted) {
+ double x = double(time - startTime) / lengthTime;
+ if (x > 1)
+ x = 1;
+ double x1 = smoothFunction(x);
+ double x0 = 1 - x1;
+ res = start * x0 + goal * x1;
+ }
return res;
}
-void SmoothReach::
-set( const dynamicgraph::Vector & goalDes, const int & lengthDes )
-{
+void SmoothReach::set(const dynamicgraph::Vector &goalDes,
+ const int &lengthDes) {
goal = goalDes;
lengthTime = lengthDes;
isParam = true;
}
-const dynamicgraph::Vector & SmoothReach::
-getGoal( void ) { return goal; }
+const dynamicgraph::Vector &SmoothReach::getGoal(void) { return goal; }
-const int & SmoothReach::
-getLength( void ) { return lengthTime; }
+const int &SmoothReach::getLength(void) { return lengthTime; }
-const int & SmoothReach::
-getStart( void ) { return startTime; }
+const int &SmoothReach::getStart(void) { return startTime; }
-
-void SmoothReach::
-display(std::ostream & os) const
-{
+void SmoothReach::display(std::ostream &os) const {
os << "Status: " << isStarted << isParam << std::endl
- << "Goal: " << goal
- << "start: " << start
- << "Times: "<< startTime << " " << lengthTime << std::endl;
+ << "Goal: " << goal << "start: " << start << "Times: " << startTime << " "
+ << lengthTime << std::endl;
}
diff --git a/src/tools/smooth-reach.hpp b/src/tools/smooth-reach.hpp
index 76ddc23b..9f589d5b 100644
--- a/src/tools/smooth-reach.hpp
+++ b/src/tools/smooth-reach.hpp
@@ -7,77 +7,67 @@
*
*/
-#include <sot/core/smooth-reach.hh>
-#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
+#include <sot/core/debug.hh>
+#include <sot/core/smooth-reach.hh>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SmoothReach, "SmoothReach");
-SmoothReach::SmoothReach(const std::string & name)
- : Entity(name)
+SmoothReach::SmoothReach(const std::string &name)
+ : Entity(name)
- ,start(0), goal(0)
- ,startTime(-1), lengthTime(-1)
- ,isStarted(false), isParam(true)
+ ,
+ start(0), goal(0), startTime(-1), lengthTime(-1), isStarted(false),
+ isParam(true)
- , startSIN(NULL, "SmoothReach("+name+")::input(vector)::start")
- ,goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
- sotNOSIGNAL
- "SmoothReach("+name+")::output(vector)::goal")
-{
+ ,
+ startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
+ goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
+ sotNOSIGNAL "SmoothReach(" + name + ")::output(vector)::goal") {
sotDEBUGIN(5);
- signalRegistration( startSIN << goalSOUT );
+ signalRegistration(startSIN << goalSOUT);
sotDEBUGOUT(5);
}
-double smoothFunction( double x )
-{
- return x;
-}
-
+double smoothFunction(double x) { return x; }
-dynamicgraph::Vector& SmoothReach::
-goalSOUT_function( dynamicgraph::Vector & goal, const int& time)
-{
- if( isParam )
- {
- start = startSIN(time);
- startTime = time;
+dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &goal,
+ const int &time) {
+ if (isParam) {
+ start = startSIN(time);
+ startTime = time;
- assert( start.size() == goal.size() );
- isParam = false; isStarted = true;
- }
+ assert(start.size() == goal.size());
+ isParam = false;
+ isStarted = true;
+ }
- if( isReady )
- {
- double x = double(time-start)/length;
- if( x>1 ) x=1;
- double x1 = smoothFunction(x);
- double x0 = 1-x1;
- goal = start*x0 + goal*x1;
- }
+ if (isReady) {
+ double x = double(time - start) / length;
+ if (x > 1)
+ x = 1;
+ double x1 = smoothFunction(x);
+ double x0 = 1 - x1;
+ goal = start * x0 + goal * x1;
+ }
return goal;
}
-void SmoothReach::
-gset( const dynamicgraph::Vector & goalDes, const int & lengthDes )
-{
+void SmoothReach::gset(const dynamicgraph::Vector &goalDes,
+ const int &lengthDes) {
goal = goalDes;
length = lengthDes;
isParam = true;
}
-const dynamicgraph::Vector & SmoothReach::
-getGoal( void ) { return goal; }
+const dynamicgraph::Vector &SmoothReach::getGoal(void) { return goal; }
-const int & SmoothReach::
-getLength( void ) { return length; }
+const int &SmoothReach::getLength(void) { return length; }
-const int & SmoothReach::
-getStart( void ) { return start; }
+const int &SmoothReach::getStart(void) { return start; }
diff --git a/src/tools/switch.cpp b/src/tools/switch.cpp
index fe940982..e942b988 100644
--- a/src/tools/switch.cpp
+++ b/src/tools/switch.cpp
@@ -8,18 +8,20 @@
#include "type-name-helper.hh"
namespace dynamicgraph {
- namespace sot {
- template< typename Tin, typename Tout, typename Time >
- std::string VariadicAbstract<Tin,Tout,Time>::getTypeInName (void) { return TypeNameHelper<Tin>::typeName; }
- template< typename Tin, typename Tout, typename Time >
- std::string VariadicAbstract<Tin,Tout,Time>::getTypeOutName(void) { return TypeNameHelper<Tout>::typeName; }
+namespace sot {
+template <typename Tin, typename Tout, typename Time>
+std::string VariadicAbstract<Tin, Tout, Time>::getTypeInName(void) {
+ return TypeNameHelper<Tin>::typeName;
+}
+template <typename Tin, typename Tout, typename Time>
+std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) {
+ return TypeNameHelper<Tout>::typeName;
+}
- typedef Switch<Vector,int> SwitchVector;
- template<>
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (SwitchVector, "SwitchVector");
+typedef Switch<Vector, int> SwitchVector;
+template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector");
- typedef Switch<bool,int> SwitchBool;
- template<>
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (SwitchBool, "SwitchBoolean");
- } // namespace sot
+typedef Switch<bool, int> SwitchBool;
+template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
+} // namespace sot
} // namespace dynamicgraph
diff --git a/src/tools/time-stamp.cpp b/src/tools/time-stamp.cpp
index cafc6496..f548edcd 100644
--- a/src/tools/time-stamp.cpp
+++ b/src/tools/time-stamp.cpp
@@ -12,10 +12,10 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/time-stamp.hh>
-#include <sot/core/matrix-geometry.hh>
#include <dynamic-graph/factory.h>
#include <sot/core/macros-signal.hh>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/time-stamp.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
@@ -24,58 +24,52 @@
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TimeStamp,"TimeStamp");
-
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TimeStamp, "TimeStamp");
/* --- CONSTRUCTION ---------------------------------------------------- */
-TimeStamp::
-TimeStamp( const std::string& name )
- :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),
- sotNOSIGNAL,
- "TimeStamp("+name+")::output(vector2)::synchro" )
- ,timeOnceDoubleSOUT( boost::bind(&TimeStamp::getTimeStampDouble,this,
- SOT_CALL_SIG(timeSOUT,dynamicgraph::Vector),_1),
- timeSOUT,
- "TimeStamp("+name+")::output(double)::synchroDouble" )
-{
+TimeStamp::TimeStamp(const std::string &name)
+ : 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),
+ sotNOSIGNAL,
+ "TimeStamp(" + name + ")::output(vector2)::synchro"),
+ timeOnceDoubleSOUT(
+ boost::bind(&TimeStamp::getTimeStampDouble, this,
+ SOT_CALL_SIG(timeSOUT, dynamicgraph::Vector), _1),
+ timeSOUT, "TimeStamp(" + name + ")::output(double)::synchroDouble") {
sotDEBUGIN(15);
- timeSOUT.setFunction( boost::bind(&TimeStamp::getTimeStamp,this,_1,_2) );
- timeDoubleSOUT.setFunction( boost::bind(&TimeStamp::getTimeStampDouble,this,
- SOT_CALL_SIG(timeSOUT,dynamicgraph::Vector),_1) );
- timeOnceSOUT.setNeedUpdateFromAllChildren( true );
- timeOnceDoubleSOUT.setNeedUpdateFromAllChildren( true );
- signalRegistration( timeSOUT << timeDoubleSOUT
- << timeOnceSOUT << timeOnceDoubleSOUT );
+ timeSOUT.setFunction(boost::bind(&TimeStamp::getTimeStamp, this, _1, _2));
+ timeDoubleSOUT.setFunction(
+ boost::bind(&TimeStamp::getTimeStampDouble, this,
+ SOT_CALL_SIG(timeSOUT, dynamicgraph::Vector), _1));
+ timeOnceSOUT.setNeedUpdateFromAllChildren(true);
+ timeOnceDoubleSOUT.setNeedUpdateFromAllChildren(true);
+ signalRegistration(timeSOUT << timeDoubleSOUT << timeOnceSOUT
+ << timeOnceDoubleSOUT);
- gettimeofday( &val,NULL );
+ gettimeofday(&val, NULL);
sotDEBUGOUT(15);
}
/* --- DISPLAY --------------------------------------------------------- */
-void TimeStamp::
-display( std::ostream& os ) const
-{
- os << "TimeStamp <> : " << val.tv_sec << "s; "
- << val.tv_usec << "us." << std::endl;
+void TimeStamp::display(std::ostream &os) const {
+ os << "TimeStamp <> : " << val.tv_sec << "s; " << val.tv_usec << "us."
+ << std::endl;
}
/* --------------------------------------------------------------------- */
/* --- CONTROL --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-dynamicgraph::Vector& TimeStamp::
-getTimeStamp( dynamicgraph::Vector& res,const int& /*time*/ )
-{
+dynamicgraph::Vector &TimeStamp::getTimeStamp(dynamicgraph::Vector &res,
+ const int & /*time*/) {
sotDEBUGIN(15);
- gettimeofday( &val,NULL );
- if( res.size()!=2 ) res.resize(2);
-
+ gettimeofday(&val, NULL);
+ if (res.size() != 2)
+ res.resize(2);
res(0) = static_cast<double>(val.tv_sec);
res(1) = static_cast<double>(val.tv_usec);
@@ -83,14 +77,15 @@ getTimeStamp( dynamicgraph::Vector& res,const int& /*time*/ )
return res;
}
-double& TimeStamp::
-getTimeStampDouble( const dynamicgraph::Vector& vect,double& res )
-{
+double &TimeStamp::getTimeStampDouble(const dynamicgraph::Vector &vect,
+ double &res) {
sotDEBUGIN(15);
- if( offsetSet ) res = (vect(0)-offsetValue)*1000;
- else res = vect(0)*1000;
- res += vect(1)/1000;
+ if (offsetSet)
+ res = (vect(0) - offsetValue) * 1000;
+ else
+ res = vect(0) * 1000;
+ res += vect(1) / 1000;
sotDEBUGOUT(15);
return res;
}
diff --git a/src/tools/timer.cpp b/src/tools/timer.cpp
index 1723a711..bb70ba81 100644
--- a/src/tools/timer.cpp
+++ b/src/tools/timer.cpp
@@ -12,10 +12,10 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/timer.hh>
#include <dynamic-graph/linear-algebra.h>
-#include <sot/core/matrix-geometry.hh>
#include <sot/core/factory.hh>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/timer.hh>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
@@ -25,52 +25,49 @@ 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");
+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,
- std::ostream& os )
-{
+void cmdChrono(const std::string &cmdLine, std::istringstream &cmdArg,
+ std::ostream &os) {
sotDEBUGIN(15);
- if( cmdLine == "help" )
- {
- os << " - chrono <cmd...>"
- << "\t\t\t\tLaunch <cmd> and display the time spent in the operation." <<std::endl;
- return;
- }
+ if (cmdLine == "help") {
+ os << " - chrono <cmd...>"
+ << "\t\t\t\tLaunch <cmd> and display the time spent in the operation."
+ << std::endl;
+ return;
+ }
- struct timeval t0,t1;
+ struct timeval t0, t1;
double dt;
- gettimeofday(&t0,NULL);
- sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
+ gettimeofday(&t0, NULL);
+ sotDEBUG(15) << "t0: " << t0.tv_sec << " - " << t0.tv_usec << std::endl;
- std::string cmdLine2; cmdArg>>cmdLine2;
- sotDEBUG(5)<<"Chrono <" <<cmdLine2<<">"<<std::endl;
+ std::string cmdLine2;
+ cmdArg >> cmdLine2;
+ sotDEBUG(5) << "Chrono <" << cmdLine2 << ">" << std::endl;
// Florent: remove reference to g_shell
- //g_shell.cmd(cmdLine2,cmdArg,os);
-
- gettimeofday(&t1,NULL);
- dt = ( (static_cast<double>(t1.tv_sec)-static_cast<double>(t0.tv_sec)) * 1000.
- + (static_cast<double>(t1.tv_usec)-static_cast<double>(t0.tv_usec)+0.) / 1000. );
- sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
+ // g_shell.cmd(cmdLine2,cmdArg,os);
+
+ gettimeofday(&t1, NULL);
+ dt = ((static_cast<double>(t1.tv_sec) - static_cast<double>(t0.tv_sec)) *
+ 1000. +
+ (static_cast<double>(t1.tv_usec) - static_cast<double>(t0.tv_usec) +
+ 0.) /
+ 1000.);
+ sotDEBUG(15) << "t1: " << t1.tv_sec << " - " << t1.tv_usec << std::endl;
os << "Time spent = " << dt << " ms " << std::endl;
diff --git a/src/tools/trajectory.cpp b/src/tools/trajectory.cpp
index d85ffde5..560cb686 100644
--- a/src/tools/trajectory.cpp
+++ b/src/tools/trajectory.cpp
@@ -7,8 +7,8 @@
*/
#define VP_DEBUG
#define VP_DEBUG_MODE 45
-#include <sot/core/debug.hh>
#include <iostream>
+#include <sot/core/debug.hh>
// #ifdef VP_DEBUG
// class sotJTE__INIT
// {
@@ -29,89 +29,76 @@
namespace dynamicgraph {
namespace sot {
-RulesJointTrajectory::RulesJointTrajectory(Trajectory &aTrajectoryToFill):
- TrajectoryToFill_(aTrajectoryToFill),
- dbg_level(0),
- float_str_re("[-0-9]+\\.[0-9]*"),
-
- // Header Regular Expression
- seq_str_re("([0-9]+)"),
- timestamp_str_re("("+float_str_re+"),("+float_str_re+")"),
- frame_id_str_re("[a-zA-z_0-9]*"),
- header_str_re("\\("+seq_str_re+"\\,\\("+timestamp_str_re+"\\),("+frame_id_str_re+")\\)\\,\\("),
-
- // List of Joint Names
- joint_name_str_re("([a-zA-Z0-9_]+)"),
- list_of_jn_str_re(joint_name_str_re + "(\\,|\\))"),
-
- // 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("\\,\\("),
-
- // 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),
- bg_liste_of_pts_re( bg_liste_of_pts_str_re)
-{
-}
+RulesJointTrajectory::RulesJointTrajectory(Trajectory &aTrajectoryToFill)
+ : TrajectoryToFill_(aTrajectoryToFill), dbg_level(0),
+ float_str_re("[-0-9]+\\.[0-9]*"),
+
+ // Header Regular Expression
+ seq_str_re("([0-9]+)"),
+ timestamp_str_re("(" + float_str_re + "),(" + float_str_re + ")"),
+ frame_id_str_re("[a-zA-z_0-9]*"),
+ header_str_re("\\(" + seq_str_re + "\\,\\(" + timestamp_str_re + "\\),(" +
+ frame_id_str_re + ")\\)\\,\\("),
+
+ // List of Joint Names
+ joint_name_str_re("([a-zA-Z0-9_]+)"),
+ list_of_jn_str_re(joint_name_str_re + "(\\,|\\))"),
+
+ // 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("\\,\\("),
-bool RulesJointTrajectory::
-search_exp_sub_string(std::string &text,
- boost::match_results<std::string::const_iterator> &what,
- boost::regex &e,
- std::string &sub_text)
-{
- unsigned nb_failures=0;
+ // 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),
+ bg_liste_of_pts_re(bg_liste_of_pts_str_re) {}
+
+bool RulesJointTrajectory::search_exp_sub_string(
+ std::string &text, boost::match_results<std::string::const_iterator> &what,
+ boost::regex &e, std::string &sub_text) {
+ unsigned nb_failures = 0;
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 ;
- for(unsigned int i = 0; i < what.size(); ++i)
- std::cout << " $" << i << " = \""
- << what[i] << "\" "
- << what.position(i) << " "
- << what.length(i) << "\n";
+ if (boost::regex_search(text, what, e, flags)) {
+
+ if (dbg_level > 5) {
+ std::cout << "** Match found **\n Sub-Expressions:" << what.size()
+ << std::endl;
+ for (unsigned int i = 0; i < what.size(); ++i)
+ std::cout << " $" << i << " = \"" << what[i] << "\" "
+ << what.position(i) << " " << what.length(i) << "\n";
}
- if (what.size()>=1)
- {
+ if (what.size() >= 1) {
unsigned int all_text = 0;
- boost::match_results<std::string::const_iterator>::difference_type pos = what.position(all_text);
- boost::match_results<std::string::const_iterator>::difference_type len = what.length(all_text);
- sub_text = text.substr(pos+len);
+ boost::match_results<std::string::const_iterator>::difference_type pos =
+ what.position(all_text);
+ boost::match_results<std::string::const_iterator>::difference_type len =
+ what.length(all_text);
+ sub_text = text.substr(pos + len);
return true;
}
- }
- else
- {
- if (dbg_level>5)
+ } else {
+ if (dbg_level > 5)
std::cout << "** No Match found **\n";
sub_text = text;
nb_failures++;
- if (nb_failures>100)
+ if (nb_failures > 100)
return false;
}
return false;
}
-void RulesJointTrajectory::
-parse_header(std::string &trajectory,
- std::string &sub_text1)
-{
+void RulesJointTrajectory::parse_header(std::string &trajectory,
+ std::string &sub_text1) {
std::istringstream is;
boost::match_results<std::string::const_iterator> what;
- if (search_exp_sub_string(trajectory,what,header_re,sub_text1))
- {
+ if (search_exp_sub_string(trajectory, what, header_re, sub_text1)) {
is.str(what[1]);
is >> TrajectoryToFill_.header_.seq_;
is.str(what[2]);
@@ -120,252 +107,218 @@ parse_header(std::string &trajectory,
is >> TrajectoryToFill_.header_.stamp_.nsecs_;
TrajectoryToFill_.header_.frame_id_ = what[4];
- if (dbg_level>5)
- {
+ if (dbg_level > 5) {
std::cout << "seq: " << TrajectoryToFill_.header_.seq_ << std::endl;
- std::cout << "ts:" << TrajectoryToFill_.header_.stamp_.secs_ << " " << what[2]
- << " " << TrajectoryToFill_.header_.stamp_.nsecs_ << " " << is.str() << std::endl;
- std::cout << "frame_id:" << TrajectoryToFill_.header_.frame_id_ << std::endl;
+ std::cout << "ts:" << TrajectoryToFill_.header_.stamp_.secs_ << " "
+ << what[2] << " " << TrajectoryToFill_.header_.stamp_.nsecs_
+ << " " << is.str() << std::endl;
+ std::cout << "frame_id:" << TrajectoryToFill_.header_.frame_id_
+ << std::endl;
- std::cout << "sub_text1:" <<sub_text1 << std::endl;
+ std::cout << "sub_text1:" << sub_text1 << std::endl;
}
}
}
-void RulesJointTrajectory::
-parse_joint_names(std::string &trajectory,
- std::string &sub_text1,
- std::vector<std::string> &joint_names)
-{
+void RulesJointTrajectory::parse_joint_names(
+ std::string &trajectory, std::string &sub_text1,
+ std::vector<std::string> &joint_names) {
std::istringstream is;
boost::match_results<std::string::const_iterator> what;
- bool joint_names_loop=true;
- do
- {
- if (search_exp_sub_string(trajectory,what,list_of_jn_re,sub_text1))
- {
+ bool joint_names_loop = true;
+ do {
+ if (search_exp_sub_string(trajectory, what, list_of_jn_re, sub_text1)) {
std::string joint_name;
is.str(what[1]);
- joint_name=what[1];
+ joint_name = what[1];
joint_names.push_back(joint_name);
std::string sep_char;
sep_char = what[2];
- if (sep_char==")")
- joint_names_loop=false;
- if (dbg_level>5)
- {
- std::cout << "joint_name:" << joint_name
- << " " << sep_char << std::endl;
- std::cout << "sub_text1:" <<sub_text1 << std::endl;
+ if (sep_char == ")")
+ joint_names_loop = false;
+ if (dbg_level > 5) {
+ std::cout << "joint_name:" << joint_name << " " << sep_char
+ << std::endl;
+ std::cout << "sub_text1:" << sub_text1 << std::endl;
}
}
- trajectory=sub_text1;
-
- }
- while(joint_names_loop);
+ trajectory = sub_text1;
+ } while (joint_names_loop);
}
-bool RulesJointTrajectory::
-parse_seq(std::string &trajectory,
- std::string &sub_text1,
- std::vector<double> &seq)
-{
+bool RulesJointTrajectory::parse_seq(std::string &trajectory,
+ std::string &sub_text1,
+ std::vector<double> &seq) {
boost::match_results<std::string::const_iterator> what;
bool joint_seq_loop = true;
std::istringstream is;
- std::string sub_text2= trajectory;
- sub_text1=trajectory;
- do
- {
- if (search_exp_sub_string(sub_text2,what,
- list_of_pv_re,
- sub_text1))
- {
+ std::string sub_text2 = trajectory;
+ sub_text1 = trajectory;
+ do {
+ if (search_exp_sub_string(sub_text2, what, list_of_pv_re, sub_text1)) {
std::string sep_char;
- if (dbg_level>5)
- {
+ if (dbg_level > 5) {
std::cout << "size:" << what.size() << std::endl;
}
- if (what.size()==3)
- {
+ if (what.size() == 3) {
std::string aString(what[1]);
- if (aString.size()>0)
- {
- is.clear();
- is.str(aString);
- double aValue;
- is >> aValue;
- if (dbg_level>5)
- { std::cout << aString << " | " << aValue << std::endl; }
-
- seq.push_back(aValue);
+ if (aString.size() > 0) {
+ is.clear();
+ is.str(aString);
+ double aValue;
+ is >> aValue;
+ if (dbg_level > 5) {
+ std::cout << aString << " | " << aValue << std::endl;
}
+
+ seq.push_back(aValue);
+ }
sep_char = what[2];
- }
- else if (what.size()==1)
+ } else if (what.size() == 1)
sep_char = what[0];
- if (sep_char==")")
- joint_seq_loop=false;
+ if (sep_char == ")")
+ joint_seq_loop = false;
- }
- else
- {
+ } else {
return true;
}
- sub_text2 =sub_text1;
- }
- while(joint_seq_loop);
+ sub_text2 = sub_text1;
+ } while (joint_seq_loop);
return true;
}
-bool
-RulesJointTrajectory::
-parse_point(std::string &trajectory,
- std::string &sub_text1)
-{
+bool RulesJointTrajectory::parse_point(std::string &trajectory,
+ std::string &sub_text1) {
std::vector<double> position, velocities, acceleration, effort;
std::string sub_text2;
boost::match_results<std::string::const_iterator> what;
JointTrajectoryPoint aJTP;
- if (!search_exp_sub_string(trajectory,what,bg_pt_re,sub_text1)) return false;
- sub_text2=sub_text1;
+ if (!search_exp_sub_string(trajectory, what, bg_pt_re, sub_text1))
+ return false;
+ sub_text2 = sub_text1;
- if (!parse_seq(sub_text2,sub_text1,aJTP.positions_)) return false;
- sub_text2= sub_text1;
+ 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 (!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;
- sub_text2= sub_text1;
+ 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 (!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;
- sub_text2= sub_text1;
+ 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 (!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;
TrajectoryToFill_.points_.push_back(aJTP);
return true;
}
-bool
-RulesJointTrajectory::
-parse_points(std::string &trajectory,
- std::string &sub_text1)
-{
+bool RulesJointTrajectory::parse_points(std::string &trajectory,
+ std::string &sub_text1) {
boost::match_results<std::string::const_iterator> what;
bool joint_points_loop = true;
std::istringstream is;
- if (!search_exp_sub_string(trajectory,what,bg_liste_of_pts_re,sub_text1)) return false;
- std::string sub_text2=sub_text1;
+ if (!search_exp_sub_string(trajectory, what, bg_liste_of_pts_re, sub_text1))
+ return false;
+ std::string sub_text2 = sub_text1;
- do
- {
- if (!search_exp_sub_string(sub_text2,what,bg_pt_re,sub_text1))
+ do {
+ if (!search_exp_sub_string(sub_text2, what, bg_pt_re, sub_text1))
return false;
- sub_text2=sub_text1;
+ sub_text2 = sub_text1;
- if (!parse_point(sub_text2,sub_text1)) return false;
- sub_text2=sub_text1;
+ 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))
+ if (!search_exp_sub_string(sub_text2, what, end_pt_re, sub_text1))
return false;
- sub_text2=sub_text1;
+ sub_text2 = sub_text1;
- if (!search_exp_sub_string(sub_text2,what,list_of_pv_re,sub_text1))
+ if (!search_exp_sub_string(sub_text2, what, list_of_pv_re, sub_text1))
return false;
- sub_text2=sub_text1;
+ sub_text2 = sub_text1;
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);
+ } while (joint_points_loop);
return true;
}
-void
-RulesJointTrajectory::
-parse_string(std::string &atext)
-{
- std::string sub_text1,sub_text2;
- parse_header(atext,sub_text2);
- sub_text1=sub_text2;
+void RulesJointTrajectory::parse_string(std::string &atext) {
+ std::string sub_text1, sub_text2;
+ parse_header(atext, sub_text2);
+ sub_text1 = sub_text2;
- parse_joint_names(sub_text1,sub_text2,TrajectoryToFill_.joint_names_);
+ parse_joint_names(sub_text1, sub_text2, TrajectoryToFill_.joint_names_);
- if (dbg_level>5)
- {
- for(std::vector<std::string>::size_type i=0;
- i<joint_names.size();i++)
+ if (dbg_level > 5) {
+ for (std::vector<std::string>::size_type i = 0; i < joint_names.size(); i++)
std::cout << joint_names[i] << std::endl;
}
- sub_text1=sub_text2;
- parse_points(sub_text1,sub_text2);
+ sub_text1 = sub_text2;
+ parse_points(sub_text1, sub_text2);
}
+Trajectory::Trajectory(void) {}
-Trajectory::Trajectory(void)
-{
-}
-
-Trajectory::Trajectory(const Trajectory ©)
-{
+Trajectory::Trajectory(const Trajectory ©) {
header_ = copy.header_;
time_from_start_ = copy.time_from_start_;
points_ = copy.points_;
}
-Trajectory::~Trajectory(void)
-{
-}
+Trajectory::~Trajectory(void) {}
-
-int Trajectory::deserialize(std::istringstream &is)
-{
- std::string aStr=is.str();
+int Trajectory::deserialize(std::istringstream &is) {
+ std::string aStr = is.str();
RulesJointTrajectory aRJT(*this);
aRJT.parse_string(aStr);
return 0;
}
-void Trajectory::display(std::ostream& os) const
-{
- unsigned int index=0;
+void Trajectory::display(std::ostream &os) const {
+ unsigned int index = 0;
os << "-- Trajectory --" << std::endl;
for (std::vector<std::string>::const_iterator it_joint_name =
joint_names_.begin();
- it_joint_name != joint_names_.end();
- it_joint_name++,index++)
- os << "Joint("<<index<<")="<< *(it_joint_name) << std::endl;
+ it_joint_name != joint_names_.end(); it_joint_name++, index++)
+ os << "Joint(" << index << ")=" << *(it_joint_name) << std::endl;
os << " Number of points: " << points_.size() << std::endl;
- for(std::vector<JointTrajectoryPoint>::const_iterator
- it_point = points_.begin();
- it_point != points_.end();
- it_point++)
- {
+ for (std::vector<JointTrajectoryPoint>::const_iterator it_point =
+ points_.begin();
+ it_point != points_.end(); it_point++) {
it_point->display(os);
}
-
}
-} /* ! 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 44f032f4..cf9b077b 100644
--- a/src/tools/type-name-helper.hh
+++ b/src/tools/type-name-helper.hh
@@ -12,28 +12,26 @@
#include <sot/core/matrix-geometry.hh>
namespace dynamicgraph {
- namespace sot {
- template< typename TypeRef >
- struct TypeNameHelper
- {
- static const std::string typeName;
- };
- template< typename TypeRef >
- const std::string TypeNameHelper<TypeRef>::typeName = "unspecified";
+namespace sot {
+template <typename TypeRef> struct TypeNameHelper {
+ static const std::string typeName;
+};
+template <typename TypeRef>
+const std::string TypeNameHelper<TypeRef>::typeName = "unspecified";
-#define ADD_KNOWN_TYPE( typeid ) \
- template<>const std::string TypeNameHelper<typeid>::typeName = #typeid
+#define ADD_KNOWN_TYPE(typeid) \
+ template <> const std::string TypeNameHelper<typeid>::typeName = #typeid
- ADD_KNOWN_TYPE(bool);
- ADD_KNOWN_TYPE(double);
- ADD_KNOWN_TYPE(Vector);
- ADD_KNOWN_TYPE(Matrix);
- ADD_KNOWN_TYPE(MatrixRotation);
- ADD_KNOWN_TYPE(MatrixTwist);
- ADD_KNOWN_TYPE(MatrixHomogeneous);
- ADD_KNOWN_TYPE(VectorQuaternion);
- ADD_KNOWN_TYPE(VectorRollPitchYaw);
+ADD_KNOWN_TYPE(bool);
+ADD_KNOWN_TYPE(double);
+ADD_KNOWN_TYPE(Vector);
+ADD_KNOWN_TYPE(Matrix);
+ADD_KNOWN_TYPE(MatrixRotation);
+ADD_KNOWN_TYPE(MatrixTwist);
+ADD_KNOWN_TYPE(MatrixHomogeneous);
+ADD_KNOWN_TYPE(VectorQuaternion);
+ADD_KNOWN_TYPE(VectorRollPitchYaw);
#undef ADD_KNOWN_TYPE
- } /* namespace sot */
+} /* namespace sot */
} /* namespace dynamicgraph */
diff --git a/src/tools/utils-windows.cpp b/src/tools/utils-windows.cpp
index a964d2e9..aef60693 100644
--- a/src/tools/utils-windows.cpp
+++ b/src/tools/utils-windows.cpp
@@ -12,44 +12,40 @@
#include < Windows.h >
#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
-#define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64
+#define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64
#else
-#define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL
+#define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL
#endif
-int gettimeofday(struct timeval *tv, struct timezone *tz)
-{
- FILETIME ft;
- unsigned __int64 tmpres = 0;
- static int tzflag;
-
- if (NULL != tv)
- {
- GetSystemTimeAsFileTime(&ft);
-
- tmpres |= ft.dwHighDateTime;
- tmpres <<= 32;
- tmpres |= ft.dwLowDateTime;
-
- /*converting file time to unix epoch*/
- tmpres /= 10; /*convert into microseconds*/
- tmpres -= DELTA_EPOCH_IN_MICROSECS;
- tv->tv_sec = (long)(tmpres / 1000000UL);
- tv->tv_usec = (long)(tmpres % 1000000UL);
- }
-
- if (NULL != tz)
- {
- if (!tzflag)
- {
- _tzset();
- tzflag++;
- }
- tz->tz_minuteswest = _timezone / 60;
- tz->tz_dsttime = _daylight;
- }
-
- return 0;
+int gettimeofday(struct timeval *tv, struct timezone *tz) {
+ FILETIME ft;
+ unsigned __int64 tmpres = 0;
+ static int tzflag;
+
+ if (NULL != tv) {
+ GetSystemTimeAsFileTime(&ft);
+
+ tmpres |= ft.dwHighDateTime;
+ tmpres <<= 32;
+ tmpres |= ft.dwLowDateTime;
+
+ /*converting file time to unix epoch*/
+ tmpres /= 10; /*convert into microseconds*/
+ tmpres -= DELTA_EPOCH_IN_MICROSECS;
+ tv->tv_sec = (long)(tmpres / 1000000UL);
+ tv->tv_usec = (long)(tmpres % 1000000UL);
+ }
+
+ if (NULL != tz) {
+ if (!tzflag) {
+ _tzset();
+ tzflag++;
+ }
+ tz->tz_minuteswest = _timezone / 60;
+ tz->tz_dsttime = _daylight;
+ }
+
+ return 0;
}
#endif /*WIN32*/
diff --git a/src/traces/reader.cpp b/src/traces/reader.cpp
index bc505141..21b18aec 100644
--- a/src/traces/reader.cpp
+++ b/src/traces/reader.cpp
@@ -12,39 +12,32 @@
/* --------------------------------------------------------------------- */
/* SOT */
-#include <sot/core/reader.hh>
#include <sot/core/debug.hh>
+#include <sot/core/reader.hh>
#include <boost/bind.hpp>
-#include <sstream>
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
+#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
using namespace std;
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotReader,"Reader");
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotReader, "Reader");
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-sotReader::sotReader( const std::string n )
- :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)
-{
- signalRegistration( selectionSIN<<vectorSOUT<<matrixSOUT );
+sotReader::sotReader(const std::string n)
+ : 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) {
+ signalRegistration(selectionSIN << vectorSOUT << matrixSOUT);
selectionSIN = true;
vectorSOUT.setNeedUpdateFromAllChildren(true);
@@ -55,38 +48,38 @@ sotReader::sotReader( const std::string n )
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-void sotReader::
-load( const string& filename )
-{
+void sotReader::load(const string &filename) {
sotDEBUGIN(15);
- std::ifstream datafile( filename.c_str() );
- const unsigned int SIZE=1024;
+ std::ifstream datafile(filename.c_str());
+ const unsigned int SIZE = 1024;
char buffer[SIZE];
std::vector<double> newline;
- while( datafile.good() )
- {
- datafile.getline( buffer,SIZE );
- const unsigned int gcount = (unsigned int)(datafile.gcount());
- if( gcount>=SIZE ) { /* TODO read error, line to long. */ }
- std::istringstream iss(buffer);
- newline.clear();
- sotDEBUG(25) << "Get line = '" << buffer << "'" << std::endl;
- while( 1 )
- {
- double x; iss>>x;
- if(! iss.fail() )newline.push_back(x); else break;
- sotDEBUG(45) << "New data = " << x << std::endl;
- }
- if( newline.size()>0 )dataSet.push_back( newline );
+ while (datafile.good()) {
+ datafile.getline(buffer, SIZE);
+ const unsigned int gcount = (unsigned int)(datafile.gcount());
+ if (gcount >= SIZE) { /* TODO read error, line to long. */
+ }
+ std::istringstream iss(buffer);
+ newline.clear();
+ sotDEBUG(25) << "Get line = '" << buffer << "'" << std::endl;
+ while (1) {
+ double x;
+ iss >> x;
+ if (!iss.fail())
+ newline.push_back(x);
+ else
+ break;
+ sotDEBUG(45) << "New data = " << x << std::endl;
}
+ if (newline.size() > 0)
+ dataSet.push_back(newline);
+ }
sotDEBUGOUT(15);
}
-void sotReader::clear( void )
-{
+void sotReader::clear(void) {
sotDEBUGIN(15);
dataSet.clear();
@@ -95,58 +88,58 @@ void sotReader::clear( void )
sotDEBUGOUT(15);
}
-void sotReader::
-rewind( void )
-{
+void sotReader::rewind(void) {
sotDEBUGIN(15);
iteratorSet = false;
sotDEBUGOUT(15);
}
-dynamicgraph::Vector& sotReader::
-getNextData( dynamicgraph::Vector& res, const unsigned int time )
-{
+dynamicgraph::Vector &sotReader::getNextData(dynamicgraph::Vector &res,
+ const unsigned int time) {
sotDEBUGIN(15);
- if(! iteratorSet )
- {
- sotDEBUG(15) << "Start the list" << std::endl;
- currentData = dataSet.begin(); iteratorSet=true;
- }
- else if( currentData!=dataSet.end() ){ ++currentData; }
+ if (!iteratorSet) {
+ sotDEBUG(15) << "Start the list" << std::endl;
+ currentData = dataSet.begin();
+ iteratorSet = true;
+ } else if (currentData != dataSet.end()) {
+ ++currentData;
+ }
- if( currentData==dataSet.end() )
- {
- sotDEBUGOUT(15);
- return res;
- }
+ if (currentData == dataSet.end()) {
+ sotDEBUGOUT(15);
+ return res;
+ }
- const Flags& selection = selectionSIN(time);
- const std::vector<double> & curr = *currentData;
+ const Flags &selection = selectionSIN(time);
+ const std::vector<double> &curr = *currentData;
- unsigned int dim=0;
- for( unsigned int i=0;i<curr.size();++i ) if( selection(i) ) dim++;
+ unsigned int dim = 0;
+ for (unsigned int i = 0; i < curr.size(); ++i)
+ 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];
+ int cursor = 0;
+ for (unsigned int i = 0; i < curr.size(); ++i)
+ if (selection(i))
+ res(cursor++) = curr[i];
sotDEBUGOUT(15);
return res;
}
-dynamicgraph::Matrix& sotReader::
-getNextMatrix( dynamicgraph::Matrix& res, const unsigned int time )
-{
+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;
+ const dynamicgraph::Vector &vect = vectorSOUT(time);
+ 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);
+ 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);
sotDEBUGOUT(15);
return res;
@@ -155,40 +148,31 @@ getNextMatrix( dynamicgraph::Matrix& res, const unsigned int time )
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
-void sotReader::
-display( std::ostream& os ) const
-{
+void sotReader::display(std::ostream &os) const {
os << CLASS_NAME << " " << name << endl;
}
-
-std::ostream& operator<< ( std::ostream& os,const sotReader& t )
-{
+std::ostream &operator<<(std::ostream &os, const sotReader &t) {
t.display(os);
return os;
}
-/* --- Command line interface ------------------------------------------------------ */
-void sotReader::initCommands()
-{
+/* --- Command line interface
+ * ------------------------------------------------------ */
+void sotReader::initCommands() {
namespace dc = ::dynamicgraph::command;
- addCommand("clear",
- dc::makeCommandVoid0(*this,&sotReader::clear,
- "Clear the data loaded"));
+ addCommand("clear", dc::makeCommandVoid0(*this, &sotReader::clear,
+ "Clear the data loaded"));
addCommand("rewind",
- dc::makeCommandVoid0(*this,&sotReader::rewind,
- "Reset the iterator to the beginning of the data set"));
+ dc::makeCommandVoid0(
+ *this, &sotReader::rewind,
+ "Reset the iterator to the beginning of the data set"));
addCommand("load",
- dc::makeCommandVoid1(*this,&sotReader::load,
- "load file"));
- addCommand("resize",
- dc::makeCommandVoid2(*this,&sotReader::resize,
- " "));
+ dc::makeCommandVoid1(*this, &sotReader::load, "load file"));
+ addCommand("resize", dc::makeCommandVoid2(*this, &sotReader::resize, " "));
}
-void sotReader::resize(const int & row, const int & col)
-{
+void sotReader::resize(const int &row, const int &col) {
rows = row;
cols = col;
}
diff --git a/src/utils/stop-watch.cpp b/src/utils/stop-watch.cpp
index b00d7b35..01f4080f 100644
--- a/src/utils/stop-watch.cpp
+++ b/src/utils/stop-watch.cpp
@@ -25,203 +25,193 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#ifndef WIN32
- #include <sys/time.h>
+#include <sys/time.h>
#else
- #include <Windows.h>
- #include <iomanip>
+#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::string;
using std::ostringstream;
+using std::string;
//#define START_PROFILER(name) getProfiler().start(name)
//#define STOP_PROFILER(name) getProfiler().stop(name)
-Stopwatch& getProfiler()
-{
- static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
+Stopwatch &getProfiler() {
+ static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
return s;
}
-Stopwatch::Stopwatch(StopwatchMode _mode)
- : active(true), mode(_mode)
-{
+Stopwatch::Stopwatch(StopwatchMode _mode) : active(true), mode(_mode) {
records_of = new map<string, PerformanceData>();
}
-Stopwatch::~Stopwatch()
-{
- delete records_of;
-}
+Stopwatch::~Stopwatch() { delete records_of; }
-void Stopwatch::set_mode(StopwatchMode new_mode)
-{
- mode = new_mode;
-}
+void Stopwatch::set_mode(StopwatchMode new_mode) { mode = new_mode; }
-bool Stopwatch::performance_exists(string perf_name)
-{
+bool Stopwatch::performance_exists(string perf_name) {
return (records_of->find(perf_name) != records_of->end());
}
-long double Stopwatch::take_time()
-{
- if ( mode == CPU_TIME ) {
-
+long double Stopwatch::take_time() {
+ if (mode == CPU_TIME) {
+
// Use ctime
return clock();
-
- } else if ( mode == REAL_TIME ) {
-
+
+ } else if (mode == REAL_TIME) {
+
// Query operating system
-
+
#ifdef WIN32
/* In case of usage under Windows */
FILETIME ft;
LARGE_INTEGER intervals;
-
+
// Get the amount of 100 nanoseconds intervals elapsed since January 1, 1601
// (UTC)
GetSystemTimeAsFileTime(&ft);
intervals.LowPart = ft.dwLowDateTime;
intervals.HighPart = ft.dwHighDateTime;
-
+
long double measure = intervals.QuadPart;
measure -= 116444736000000000.0; // Convert to UNIX epoch time
measure /= 10000000.0; // Convert to seconds
-
+
return measure;
#else
/* Linux, MacOS, ... */
struct timeval tv;
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
-
+
} else {
// If mode == NONE, clock has not been initialized, then throw exception
throw StopwatchException("Clock not initialized to a time taking mode!");
}
}
-void Stopwatch::start(string perf_name)
-{
- if (!active) return;
-
+void Stopwatch::start(string perf_name) {
+ if (!active)
+ return;
+
// Just works if not already present
records_of->insert(make_pair(perf_name, PerformanceData()));
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
// Take ctime
perf_info.clock_start = take_time();
-
+
// If this is a new start (i.e. not a restart)
-// if (!perf_info.paused)
-// perf_info.last_time = 0;
-
+ // if (!perf_info.paused)
+ // perf_info.last_time = 0;
+
perf_info.paused = false;
}
-void Stopwatch::stop(string perf_name)
-{
- if (!active) return;
-
+void Stopwatch::stop(string perf_name) {
+ if (!active)
+ return;
+
long double clock_end = take_time();
-
+
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
// check whether the performance has been reset
- if(perf_info.clock_start==0)
+ 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;
-
+ long double lapse = clock_end - perf_info.clock_start;
+
+ 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 )
+ if (lapse >= perf_info.max_time)
perf_info.max_time = lapse;
- if ( lapse <= perf_info.min_time || perf_info.min_time == 0 )
+ if (lapse <= perf_info.min_time || perf_info.min_time == 0)
perf_info.min_time = lapse;
-
+
// Update total time
perf_info.total_time += lapse;
}
-void Stopwatch::pause(string perf_name)
-{
- if (!active) return;
-
- long double clock_end = clock();
-
+void Stopwatch::pause(string perf_name) {
+ if (!active)
+ return;
+
+ long double clock_end = clock();
+
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
// check whether the performance has been reset
- if(perf_info.clock_start==0)
+ if (perf_info.clock_start == 0)
return;
- long double lapse = clock_end - perf_info.clock_start;
-
+ long double lapse = clock_end - perf_info.clock_start;
+
// Update total time
perf_info.last_time += lapse;
perf_info.total_time += lapse;
}
-void Stopwatch::reset_all()
-{
- if (!active) return;
-
+void Stopwatch::reset_all() {
+ if (!active)
+ return;
+
map<string, PerformanceData>::iterator it;
-
+
for (it = records_of->begin(); it != records_of->end(); ++it) {
reset(it->first);
}
}
-void Stopwatch::report_all(int precision, std::ostream& output)
-{
- if (!active) return;
-
- output<< "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - nSamples - totalTime) ***\n";
+void Stopwatch::report_all(int precision, std::ostream &output) {
+ if (!active)
+ return;
+
+ output << "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - "
+ "nSamples - totalTime) ***\n";
map<string, PerformanceData>::iterator it;
for (it = records_of->begin(); it != records_of->end(); ++it) {
report(it->first, precision, output);
}
}
-void Stopwatch::reset(string perf_name)
-{
- if (!active) return;
-
+void Stopwatch::reset(string perf_name) {
+ if (!active)
+ return;
+
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
perf_info.clock_start = 0;
perf_info.total_time = 0;
perf_info.min_time = 0;
@@ -231,117 +221,105 @@ void Stopwatch::reset(string perf_name)
perf_info.stops = 0;
}
-void Stopwatch::turn_on()
-{
+void Stopwatch::turn_on() {
std::cout << "Stopwatch active." << std::endl;
active = true;
}
-void Stopwatch::turn_off()
-{
+void Stopwatch::turn_off() {
std::cout << "Stopwatch inactive." << std::endl;
active = false;
}
-void Stopwatch::report(string perf_name, int precision, std::ostream& output)
-{
- if (!active) return;
-
+void Stopwatch::report(string perf_name, int precision, std::ostream &output) {
+ if (!active)
+ return;
+
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
string pad = "";
- for (size_t i = perf_name.length(); i<STOP_WATCH_MAX_NAME_LENGTH; i++)
+ for (size_t i = perf_name.length(); i < STOP_WATCH_MAX_NAME_LENGTH; i++)
pad.append(" ");
-
+
output << perf_name << pad;
- output << std::fixed << std::setprecision(precision)
- << (perf_info.min_time*1e3) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.total_time*1e3 / (long double) perf_info.stops) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.max_time*1e3) << "\t";
output << std::fixed << std::setprecision(precision)
- << (perf_info.last_time*1e3) << "\t";
+ << (perf_info.min_time * 1e3) << "\t";
+ output << std::fixed << std::setprecision(precision)
+ << (perf_info.total_time * 1e3 / (long double)perf_info.stops) << "\t";
output << std::fixed << std::setprecision(precision)
- << perf_info.stops << std::endl;
+ << (perf_info.max_time * 1e3) << "\t";
output << std::fixed << std::setprecision(precision)
- << perf_info.total_time*1e3 << std::endl;
+ << (perf_info.last_time * 1e3) << "\t";
+ output << std::fixed << std::setprecision(precision) << perf_info.stops
+ << std::endl;
+ output << std::fixed << std::setprecision(precision)
+ << perf_info.total_time * 1e3 << std::endl;
}
-long double Stopwatch::get_time_so_far(string perf_name)
-{
+long double Stopwatch::get_time_so_far(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- long double lapse =
- (take_time() - (records_of->find(perf_name)->second).clock_start);
-
+
+ long double lapse =
+ (take_time() - (records_of->find(perf_name)->second).clock_start);
+
if (mode == CPU_TIME)
- lapse /= (double) CLOCKS_PER_SEC;
-
+ lapse /= (double)CLOCKS_PER_SEC;
+
return lapse;
}
-long double Stopwatch::get_total_time(string perf_name)
-{
+long double Stopwatch::get_total_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
return perf_info.total_time;
-
}
-long double Stopwatch::get_average_time(string perf_name)
-{
+long double Stopwatch::get_average_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
return (perf_info.total_time / (long double)perf_info.stops);
-
}
-long double Stopwatch::get_min_time(string perf_name)
-{
+long double Stopwatch::get_min_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
return perf_info.min_time;
-
}
-long double Stopwatch::get_max_time(string perf_name)
-{
+long double Stopwatch::get_max_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
return perf_info.max_time;
-
}
-long double Stopwatch::get_last_time(string perf_name)
-{
+long double Stopwatch::get_last_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
+ if (!performance_exists(perf_name))
throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
+
+ PerformanceData &perf_info = records_of->find(perf_name)->second;
+
return perf_info.last_time;
}
diff --git a/unitTesting/control/test_control_pd.cpp b/unitTesting/control/test_control_pd.cpp
index 82edfb00..4bee6dac 100644
--- a/unitTesting/control/test_control_pd.cpp
+++ b/unitTesting/control/test_control_pd.cpp
@@ -9,28 +9,26 @@
#include <iostream>
#include <sot/core/debug.hh>
-
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
#include <sot/core/control-pd.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-#define BOOST_TEST_MODULE debug-control-pd
+#define BOOST_TEST_MODULE debug - control - pd
-#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
+#include <boost/test/unit_test.hpp>
-BOOST_AUTO_TEST_CASE(control_pd)
-{
+BOOST_AUTO_TEST_CASE(control_pd) {
sot::ControlPD *aControlPD = new ControlPD("acontrol_pd");
aControlPD->init(0.001);
std::istringstream Kpiss("[5](10.0,20.0,30.0,40.0,50.0)");
@@ -60,10 +58,8 @@ BOOST_AUTO_TEST_CASE(control_pd)
BOOST_CHECK(output.is_equal(" 2\n2.1\n2.2\n2.3\n2.4\n"));
}
{
- boost::test_tools::output_test_stream output;
+ boost::test_tools::output_test_stream output;
aControlPD->velocityErrorSOUT.get(output);
BOOST_CHECK(output.is_equal("1.5\n1.4\n1.3\n1.2\n1.1\n"));
}
}
-
-
diff --git a/unitTesting/dummy.cpp b/unitTesting/dummy.cpp
index cce7fc21..f9ac5d6b 100644
--- a/unitTesting/dummy.cpp
+++ b/unitTesting/dummy.cpp
@@ -10,8 +10,7 @@
#include <iostream>
#include <sot/core/debug.hh>
-int main (int , char** )
-{
+int main(int, char **) {
dynamicgraph::sot::sotDEBUGFLOW.openFile();
dynamicgraph::sot::sotDEBUGFLOW.trace("test test test");
diff --git a/unitTesting/factory/test_factory.cpp b/unitTesting/factory/test_factory.cpp
index 00208d61..03209df7 100644
--- a/unitTesting/factory/test_factory.cpp
+++ b/unitTesting/factory/test_factory.cpp
@@ -10,16 +10,16 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <string>
#include <iostream>
+#include <string>
-#include <sot/core/factory.hh>
+#include "../test-paths.h"
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
-#include "../test-paths.h"
-#include <sot/core/feature-visual-point.hh>
-#include <sot/core/exception-feature.hh>
#include <sot/core/debug.hh>
+#include <sot/core/exception-feature.hh>
+#include <sot/core/factory.hh>
+#include <sot/core/feature-visual-point.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dg;
@@ -31,115 +31,113 @@ using namespace dg;
#endif
#ifdef WIN32
- typedef HMODULE sotPluginKey;
+typedef HMODULE sotPluginKey;
#else
- typedef void* sotPluginKey;
+typedef void *sotPluginKey;
#endif
-class TestFeature
- :public FeatureAbstract
-{
+class TestFeature : public FeatureAbstract {
public:
- TestFeature( void ) : FeatureAbstract("") {}
- virtual ~TestFeature( void ) {}
- virtual unsigned int& getDimension( unsigned int& res,int /*time*/ ) {return res;}
+ TestFeature(void) : FeatureAbstract("") {}
+ virtual ~TestFeature(void) {}
+ virtual unsigned int &getDimension(unsigned int &res, int /*time*/) {
+ return res;
+ }
- virtual dg::Vector& computeError( dg::Vector& res, int /*time*/ ) {return res;}
- virtual dg::Matrix& computeJacobian( dg::Matrix& res, int /*time*/ ) {return res;}
- virtual dg::Vector& computeActivation( dg::Vector& res, int /*time*/ ) {return res;}
+ virtual dg::Vector &computeError(dg::Vector &res, int /*time*/) {
+ return res;
+ }
+ virtual dg::Matrix &computeJacobian(dg::Matrix &res, int /*time*/) {
+ return res;
+ }
+ virtual dg::Vector &computeActivation(dg::Vector &res, int /*time*/) {
+ return res;
+ }
};
+int main() {
-int main()
-{
-
- sotDEBUG(0) << "# In {"<<endl;
-// Entity test("");
-// ExceptionFeature t2(ExceptionFeature::BAD_INIT);
-// ExceptionSignal t4(ExceptionSignal::COPY_NOT_INITIALIZED);
-// Flags t3;
-// TestFeature t5;
+ sotDEBUG(0) << "# In {" << endl;
+ // Entity test("");
+ // ExceptionFeature t2(ExceptionFeature::BAD_INIT);
+ // ExceptionSignal t4(ExceptionSignal::COPY_NOT_INITIALIZED);
+ // Flags t3;
+ // TestFeature t5;
#ifdef WIN32
- sotPluginKey dlib = LoadLibrary (PLUGIN_LIB_INSTALL_PATH "/feature-visual-point" TESTS_DYNLIBSUFFIX);
+ sotPluginKey dlib = LoadLibrary(PLUGIN_LIB_INSTALL_PATH
+ "/feature-visual-point" TESTS_DYNLIBSUFFIX);
#else
- sotPluginKey dlib = dlopen(PLUGIN_LIB_INSTALL_PATH "/feature-visual-point" TESTS_DYNLIBSUFFIX, RTLD_NOW);
+ sotPluginKey dlib =
+ dlopen(PLUGIN_LIB_INSTALL_PATH "/feature-visual-point" TESTS_DYNLIBSUFFIX,
+ RTLD_NOW);
#endif
-if( NULL==dlib )
- {
- cerr << " Error dl"<<endl;
+ if (NULL == dlib) {
+ cerr << " Error dl" << endl;
#ifndef WIN32
- cerr << dlerror() <<endl;
+ cerr << dlerror() << endl;
#else
// Retrieve the system error message for the last-error code
LPTSTR pszMessage;
DWORD dwLastError = GetLastError();
- FormatMessage(
- FORMAT_MESSAGE_ALLOCATE_BUFFER |
- FORMAT_MESSAGE_FROM_SYSTEM |
- FORMAT_MESSAGE_IGNORE_INSERTS,
- NULL,
- dwLastError,
- MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
- (LPTSTR)&pszMessage,
- 0, NULL );
-
-
- cerr << pszMessage <<endl;
- LocalFree(pszMessage);
-#endif
+ FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM |
+ FORMAT_MESSAGE_IGNORE_INSERTS,
+ NULL, dwLastError, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+ (LPTSTR)&pszMessage, 0, NULL);
- exit(1);
- }
+ cerr << pszMessage << endl;
+ LocalFree(pszMessage);
+#endif
+ exit(1);
+ }
#ifdef WIN32
- dlib = LoadLibrary (PLUGIN_LIB_INSTALL_PATH "/gain-adaptive" TESTS_DYNLIBSUFFIX);
+ dlib =
+ LoadLibrary(PLUGIN_LIB_INSTALL_PATH "/gain-adaptive" TESTS_DYNLIBSUFFIX);
#else
- dlib = dlopen(PLUGIN_LIB_INSTALL_PATH "/gain-adaptive" TESTS_DYNLIBSUFFIX, RTLD_NOW);
+ dlib = dlopen(PLUGIN_LIB_INSTALL_PATH "/gain-adaptive" TESTS_DYNLIBSUFFIX,
+ RTLD_NOW);
#endif
- if( NULL==dlib )
- {
- cerr << " Error dl"<<endl;
+ if (NULL == dlib) {
+ cerr << " Error dl" << endl;
#ifndef WIN32
- cerr << dlerror() <<endl;
+ cerr << dlerror() << endl;
#else
// Retrieve the system error message for the last-error code
LPTSTR pszMessage;
DWORD dwLastError = GetLastError();
- FormatMessage(
- FORMAT_MESSAGE_ALLOCATE_BUFFER |
- FORMAT_MESSAGE_FROM_SYSTEM |
- FORMAT_MESSAGE_IGNORE_INSERTS,
- NULL,
- dwLastError,
- MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
- (LPTSTR)&pszMessage,
- 0, NULL );
-
-
- cerr << pszMessage <<endl;
- LocalFree(pszMessage);
+ FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM |
+ FORMAT_MESSAGE_IGNORE_INSERTS,
+ NULL, dwLastError, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+ (LPTSTR)&pszMessage, 0, NULL);
+
+ cerr << pszMessage << endl;
+ LocalFree(pszMessage);
#endif
- exit(1);
- }
+ exit(1);
+ }
- Entity* gain =
- FactoryStorage::getInstance()->newEntity("GainAdaptive","Gain");
- FeatureAbstract* point = sotFactory.newFeature("FeatureVisualPoint","DynamicTest.");
+ Entity *gain =
+ FactoryStorage::getInstance()->newEntity("GainAdaptive", "Gain");
+ FeatureAbstract *point =
+ sotFactory.newFeature("FeatureVisualPoint", "DynamicTest.");
try {
- gain->display(cout); cout << endl;
- cout <<gain->getClassName(); cout << endl;
-
- point->display(cout); cout << endl;
- cout <<point->getClassName(); cout << endl;
- }
- catch ( ExceptionSignal e ) {
- cout << "Exception caught! " << e << endl;
+ gain->display(cout);
+ cout << endl;
+ cout << gain->getClassName();
+ cout << endl;
+
+ point->display(cout);
+ cout << endl;
+ cout << point->getClassName();
+ cout << endl;
+ } catch (ExceptionSignal e) {
+ cout << "Exception caught! " << e << endl;
}
- sotDEBUG(0) << "# Out }"<<endl;
+ sotDEBUG(0) << "# Out }" << endl;
}
diff --git a/unitTesting/features/test_feature_generic.cpp b/unitTesting/features/test_feature_generic.cpp
index 422f568e..1187d4b8 100644
--- a/unitTesting/features/test_feature_generic.cpp
+++ b/unitTesting/features/test_feature_generic.cpp
@@ -12,12 +12,12 @@
/* -------------------------------------------------------------------------- */
#include <iostream>
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/multibody/data.hpp>
-#include <pinocchio/algorithm/kinematics.hpp>
-#include <pinocchio/algorithm/joint-configuration.hpp>
-#include <pinocchio/algorithm/jacobian.hpp>
#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/model.hpp>
#include <pinocchio/parsers/sample-models.hpp>
#include <pinocchio/multibody/liegroup/liegroup.hpp>
@@ -25,15 +25,15 @@
#define BOOST_TEST_MODULE features
#include <boost/test/unit_test.hpp>
-#include <sot/core/sot.hh>
+#include <Eigen/SVD>
+#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/debug.hh>
+#include <sot/core/feature-abstract.hh>
#include <sot/core/feature-generic.hh>
#include <sot/core/feature-pose.hh>
-#include <sot/core/feature-abstract.hh>
-#include <sot/core/debug.hh>
-#include <sot/core/task.hh>
#include <sot/core/gain-adaptive.hh>
-#include <dynamic-graph/linear-algebra.h>
-#include <Eigen/SVD>
+#include <sot/core/sot.hh>
+#include <sot/core/task.hh>
using namespace std;
using namespace dynamicgraph::sot;
@@ -41,502 +41,480 @@ 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" << (Vb).transpose() << "\n]")
+ "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]")
-
-class FeatureTestBase
-{
- public:
- Task task_;
- int time_;
- dynamicgraph::Vector expectedTaskOutput_;
-
- FeatureTestBase(unsigned dim,const std::string &name)
- : task_("task"+name),
- time_(0)
- {
- expectedTaskOutput_.resize(dim);
- }
+ "check " #Va ".isApprox(" #Vb ") failed " \
+ "[\n" \
+ << (Va) << "\n!=\n" \
+ << (Vb) << "\n]")
+
+class FeatureTestBase {
+public:
+ Task task_;
+ int time_;
+ dynamicgraph::Vector expectedTaskOutput_;
+
+ FeatureTestBase(unsigned dim, const std::string &name)
+ : task_("task" + name), time_(0) {
+ expectedTaskOutput_.resize(dim);
+ }
- virtual void init ()
- {
- task_.addFeature(featureAbstract());
- task_.setWithDerivative(true);
- }
+ virtual void init() {
+ task_.addFeature(featureAbstract());
+ task_.setWithDerivative(true);
+ }
- void computeExpectedTaskOutput (const Vector& error, const Vector& errorDrift)
- {
- double gain = task_.controlGainSIN;
- expectedTaskOutput_ = - gain * error - errorDrift;
- }
+ void computeExpectedTaskOutput(const Vector &error,
+ const Vector &errorDrift) {
+ double gain = task_.controlGainSIN;
+ expectedTaskOutput_ = -gain * error - errorDrift;
+ }
- template <typename LG_t>
- void computeExpectedTaskOutput (const Vector& s, const Vector& sdes,
- const Vector& sDesDot, const LG_t& lg)
- {
- Vector s_sd (lg.nv());
- lg.difference (sdes, s, s_sd);
+ template <typename LG_t>
+ void computeExpectedTaskOutput(const Vector &s, const Vector &sdes,
+ const Vector &sDesDot, const LG_t &lg) {
+ Vector s_sd(lg.nv());
+ lg.difference(sdes, s, s_sd);
- Eigen::Matrix<typename LG_t::Scalar, LG_t::NV, LG_t::NV> Jminus (lg.nv(),lg.nv());
- lg.template dDifference<pinocchio::ARG0>(sdes, s, Jminus);
+ Eigen::Matrix<typename LG_t::Scalar, LG_t::NV, LG_t::NV> Jminus(lg.nv(),
+ lg.nv());
+ lg.template dDifference<pinocchio::ARG0>(sdes, s, Jminus);
- computeExpectedTaskOutput (s_sd, Jminus * sDesDot);
- }
+ computeExpectedTaskOutput(s_sd, Jminus * sDesDot);
+ }
- void checkTaskOutput ()
- {
- BOOST_REQUIRE_EQUAL (task_.taskSOUT.accessCopy().size(), expectedTaskOutput_.size());
- Vector taskOutput (expectedTaskOutput_.size());
- for( int i=0;i<expectedTaskOutput_.size(); ++i )
- taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound();
- EIGEN_VECTOR_IS_APPROX(taskOutput, expectedTaskOutput_, 1e-6);
- }
+ void checkTaskOutput() {
+ BOOST_REQUIRE_EQUAL(task_.taskSOUT.accessCopy().size(),
+ expectedTaskOutput_.size());
+ Vector taskOutput(expectedTaskOutput_.size());
+ for (int i = 0; i < expectedTaskOutput_.size(); ++i)
+ taskOutput[i] = task_.taskSOUT.accessCopy()[i].getSingleBound();
+ EIGEN_VECTOR_IS_APPROX(taskOutput, expectedTaskOutput_, 1e-6);
+ }
- void setGain (double gain)
- {
- task_.controlGainSIN = gain;
- task_.controlGainSIN.access(time_);
- task_.controlGainSIN.setReady();
- }
+ void setGain(double gain) {
+ task_.controlGainSIN = gain;
+ task_.controlGainSIN.access(time_);
+ task_.controlGainSIN.setReady();
+ }
- template <typename SignalType, typename ValueType>
- void setSignal (SignalType& sig, const ValueType& v)
- {
- sig = v;
- sig.access(time_);
- sig.setReady();
- }
+ template <typename SignalType, typename ValueType>
+ void setSignal(SignalType &sig, const ValueType &v) {
+ sig = v;
+ sig.access(time_);
+ sig.setReady();
+ }
- virtual FeatureAbstract& featureAbstract() = 0;
+ virtual FeatureAbstract &featureAbstract() = 0;
- virtual void setInputs () = 0;
+ virtual void setInputs() = 0;
- virtual void printInputs() {}
+ virtual void printInputs() {}
- virtual void recompute()
- {
- task_.taskSOUT.recompute(time_);
+ virtual void recompute() {
+ task_.taskSOUT.recompute(time_);
- // Check that recomputing went fine.
- FeatureAbstract& f (featureAbstract());
- BOOST_CHECK_EQUAL (time_, f.errorSOUT.getTime());
- BOOST_CHECK_EQUAL (time_, f.errordotSOUT.getTime());
- BOOST_CHECK_EQUAL (time_, task_.errorSOUT.getTime());
- BOOST_CHECK_EQUAL (time_, task_.errorTimeDerivativeSOUT.getTime());
+ // Check that recomputing went fine.
+ FeatureAbstract &f(featureAbstract());
+ BOOST_CHECK_EQUAL(time_, f.errorSOUT.getTime());
+ BOOST_CHECK_EQUAL(time_, f.errordotSOUT.getTime());
+ BOOST_CHECK_EQUAL(time_, task_.errorSOUT.getTime());
+ BOOST_CHECK_EQUAL(time_, task_.errorTimeDerivativeSOUT.getTime());
- //TODO check that the output is correct
- //computeExpectedTaskOutput (s - sd, - vd);
- }
+ // TODO check that the output is correct
+ // computeExpectedTaskOutput (s - sd, - vd);
+ }
- virtual void printOutputs() {}
+ virtual void printOutputs() {}
- virtual void checkValue ()
- {
- time_++;
- setInputs();
- printInputs();
- recompute();
- printOutputs();
- }
+ virtual void checkValue() {
+ time_++;
+ setInputs();
+ printInputs();
+ recompute();
+ printOutputs();
+ }
};
-class TestFeatureGeneric : public FeatureTestBase
-{
- public:
- FeatureGeneric feature_,featureDes_;
- int dim_;
-
- TestFeatureGeneric(unsigned dim,const std::string &name):
- FeatureTestBase (dim, name),
- feature_("feature"+name),
- featureDes_("featureDes"+name),
- dim_ (dim)
- {
- feature_.setReference(&featureDes_);
- feature_.selectionSIN=Flags(true);
-
- dynamicgraph::Matrix Jq(dim,dim);
- Jq.setIdentity();
- feature_.jacobianSIN.setReference(&Jq);
-
- init();
- }
+class TestFeatureGeneric : public FeatureTestBase {
+public:
+ FeatureGeneric feature_, featureDes_;
+ int dim_;
- FeatureAbstract& featureAbstract()
- {
- return feature_;
- }
+ TestFeatureGeneric(unsigned dim, const std::string &name)
+ : FeatureTestBase(dim, name), feature_("feature" + name),
+ featureDes_("featureDes" + name), dim_(dim) {
+ feature_.setReference(&featureDes_);
+ feature_.selectionSIN = Flags(true);
- void setInputs()
- {
- dynamicgraph::Vector s(dim_), sd(dim_), vd(dim_);
- 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;
- }
+ dynamicgraph::Matrix Jq(dim, dim);
+ Jq.setIdentity();
+ feature_.jacobianSIN.setReference(&Jq);
- feature_.errorSIN = s;
- feature_.errorSIN.access(time_);
- feature_.errorSIN.setReady();
+ init();
+ }
- featureDes_.errorSIN = sd;
- featureDes_.errorSIN.access(time_);
- featureDes_.errorSIN.setReady();
+ FeatureAbstract &featureAbstract() { return feature_; }
+
+ void setInputs() {
+ dynamicgraph::Vector s(dim_), sd(dim_), vd(dim_);
+ 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;
+ }
- featureDes_.errordotSIN = vd;
- featureDes_.errordotSIN.access(time_);
- featureDes_.errordotSIN.setReady();
+ feature_.errorSIN = s;
+ feature_.errorSIN.access(time_);
+ feature_.errorSIN.setReady();
- setGain(gain);
- }
+ featureDes_.errorSIN = sd;
+ featureDes_.errorSIN.access(time_);
+ featureDes_.errorSIN.setReady();
- void printInputs()
- {
- BOOST_TEST_MESSAGE("----- inputs -----");
- BOOST_TEST_MESSAGE("feature_.errorSIN: " << feature_.errorSIN(time_).transpose());
- BOOST_TEST_MESSAGE("featureDes_.errorSIN: " << featureDes_.errorSIN(time_).transpose());
- BOOST_TEST_MESSAGE("featureDes_.errordotSIN: " << featureDes_.errordotSIN(time_).transpose());
- BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
- }
+ featureDes_.errordotSIN = vd;
+ featureDes_.errordotSIN.access(time_);
+ featureDes_.errordotSIN.setReady();
- void recompute()
- {
- FeatureTestBase::recompute ();
+ setGain(gain);
+ }
- dynamicgraph::Vector s = feature_.errorSIN;
- dynamicgraph::Vector sd = featureDes_.errorSIN;
- dynamicgraph::Vector vd = featureDes_.errordotSIN;
+ void printInputs() {
+ BOOST_TEST_MESSAGE("----- inputs -----");
+ BOOST_TEST_MESSAGE(
+ "feature_.errorSIN: " << feature_.errorSIN(time_).transpose());
+ BOOST_TEST_MESSAGE(
+ "featureDes_.errorSIN: " << featureDes_.errorSIN(time_).transpose());
+ BOOST_TEST_MESSAGE("featureDes_.errordotSIN: "
+ << featureDes_.errordotSIN(time_).transpose());
+ BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
+ }
- computeExpectedTaskOutput (s - sd, - vd);
+ void recompute() {
+ FeatureTestBase::recompute();
- checkTaskOutput();
- }
+ dynamicgraph::Vector s = feature_.errorSIN;
+ dynamicgraph::Vector sd = featureDes_.errorSIN;
+ dynamicgraph::Vector vd = featureDes_.errordotSIN;
- void printOutputs()
- {
- BOOST_TEST_MESSAGE("----- output -----");
- BOOST_TEST_MESSAGE("time: " << time_);
- BOOST_TEST_MESSAGE("feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
- BOOST_TEST_MESSAGE("feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
- BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
- //BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
- //BOOST_TEST_MESSAGE("task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_));
- BOOST_TEST_MESSAGE("Expected task output: " << expectedTaskOutput_.transpose());
- }
+ computeExpectedTaskOutput(s - sd, -vd);
+
+ checkTaskOutput();
+ }
+
+ void printOutputs() {
+ BOOST_TEST_MESSAGE("----- output -----");
+ BOOST_TEST_MESSAGE("time: " << time_);
+ BOOST_TEST_MESSAGE(
+ "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
+ BOOST_TEST_MESSAGE(
+ "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
+ BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
+ // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
+ // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
+ // task_.errorTimeDerivativeSOUT(time_));
+ BOOST_TEST_MESSAGE(
+ "Expected task output: " << expectedTaskOutput_.transpose());
+ }
};
BOOST_AUTO_TEST_SUITE(feature_generic)
-BOOST_AUTO_TEST_CASE (check_value)
-{
+BOOST_AUTO_TEST_CASE(check_value) {
std::string srobot("test");
- unsigned int dim=6;
-
- TestFeatureGeneric testFeatureGeneric(dim,srobot);
-
+ unsigned int dim = 6;
+
+ TestFeatureGeneric testFeatureGeneric(dim, srobot);
+
for (int i = 0; i < 10; ++i)
testFeatureGeneric.checkValue();
}
BOOST_AUTO_TEST_SUITE_END() // feature_generic
-MatrixHomogeneous randomM()
-{
+MatrixHomogeneous randomM() {
MatrixHomogeneous M;
M.translation().setRandom();
- Eigen::Vector3d w (Eigen::Vector3d::Random());
- M.linear() = Eigen::AngleAxisd (w.norm(), w.normalized()).matrix();
+ Eigen::Vector3d w(Eigen::Vector3d::Random());
+ M.linear() = Eigen::AngleAxisd(w.norm(), w.normalized()).matrix();
return M;
}
typedef pinocchio::SE3 SE3;
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t;
-typedef pinocchio::CartesianProductOperation <
- pinocchio::VectorSpaceOperationTpl<3, double>,
- pinocchio::SpecialOrthogonalOperationTpl<3, double> > R3xSO3_t;
-template <Representation_t representation>
-struct LG_t
-{
- typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t, R3xSO3_t>::type type;
+typedef pinocchio::CartesianProductOperation<
+ pinocchio::VectorSpaceOperationTpl<3, double>,
+ pinocchio::SpecialOrthogonalOperationTpl<3, double>>
+ R3xSO3_t;
+template <Representation_t representation> struct LG_t {
+ typedef typename boost::mpl::if_c<representation == SE3Representation, SE3_t,
+ R3xSO3_t>::type type;
};
-Vector7 toVector (const pinocchio::SE3& M)
-{
+Vector7 toVector(const pinocchio::SE3 &M) {
Vector7 v;
v.head<3>() = M.translation();
QuaternionMap(v.tail<4>().data()) = M.rotation();
return v;
}
-Vector toVector (const std::vector<MultiBound>& in)
-{
- Vector out (in.size());
+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();
return out;
}
template <Representation_t representation>
-class TestFeaturePose : public FeatureTestBase
-{
- public:
+class TestFeaturePose : public FeatureTestBase {
+public:
typedef typename LG_t<representation>::type LieGroup_t;
FeaturePose<representation> feature_;
bool relative_;
pinocchio::Model model_;
- pinocchio::Data data_;
+ pinocchio::Data data_;
pinocchio::JointIndex ja_, jb_;
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
+ TestFeaturePose(bool relative, const std::string &name)
+ : 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 ("rarmwrist2_joint");
- fb_ = model_.addFrame (pinocchio::Model::Frame (
- "frame_b",
- jb_,
- model_.getFrameId ("rarmwrist2_joint", pinocchio::JOINT),
- SE3::Identity(),
- pinocchio::OP_FRAME));
+ model_.upperPositionLimit.head<3>().setConstant(1.);
+ jb_ = model_.getJointId("rarmwrist2_joint");
+ fb_ = model_.addFrame(pinocchio::Model::Frame(
+ "frame_b", jb_, model_.getFrameId("rarmwrist2_joint", pinocchio::JOINT),
+ SE3::Identity(), pinocchio::OP_FRAME));
if (relative) {
- ja_ = model_.getJointId ("larmwrist2_joint");
- fa_ = model_.addFrame (pinocchio::Model::Frame (
- "frame_a",
- ja_,
- model_.getFrameId ("larmwrist2_joint", pinocchio::JOINT),
- SE3::Identity(),
- pinocchio::OP_FRAME));
+ ja_ = model_.getJointId("larmwrist2_joint");
+ fa_ = model_.addFrame(pinocchio::Model::Frame(
+ "frame_a", ja_,
+ model_.getFrameId("larmwrist2_joint", pinocchio::JOINT),
+ SE3::Identity(), pinocchio::OP_FRAME));
} else {
ja_ = 0;
- fa_ = model_.addFrame (pinocchio::Model::Frame (
- "frame_a",
- 0,
- 0,
- SE3::Identity(),
- pinocchio::OP_FRAME));
+ fa_ = model_.addFrame(pinocchio::Model::Frame(
+ "frame_a", 0, 0, SE3::Identity(), pinocchio::OP_FRAME));
}
- data_ = pinocchio::Data (model_);
+ data_ = pinocchio::Data(model_);
init();
setJointFrame();
}
- void _setFrame ()
- {
- setSignal (feature_.jaMfa,
+ void _setFrame() {
+ setSignal(
+ feature_.jaMfa,
MatrixHomogeneous(model_.frames[fa_].placement.toHomogeneousMatrix()));
- setSignal (feature_.jbMfb,
+ setSignal(
+ feature_.jbMfb,
MatrixHomogeneous(model_.frames[fb_].placement.toHomogeneousMatrix()));
}
- void setJointFrame ()
- {
+ void setJointFrame() {
model_.frames[fa_].placement.setIdentity();
model_.frames[fb_].placement.setIdentity();
_setFrame();
}
- void setRandomFrame ()
- {
+ void setRandomFrame() {
model_.frames[fa_].placement.setRandom();
model_.frames[fb_].placement.setRandom();
_setFrame();
}
- FeatureAbstract& featureAbstract()
- {
- return feature_;
- }
+ FeatureAbstract &featureAbstract() { return feature_; }
- void setInputs()
- {
- Vector q (pinocchio::randomConfiguration (model_));
- pinocchio::framesForwardKinematics (model_, data_, q);
- pinocchio::computeJointJacobians (model_, data_);
+ void setInputs() {
+ Vector q(pinocchio::randomConfiguration(model_));
+ pinocchio::framesForwardKinematics(model_, data_, q);
+ pinocchio::computeJointJacobians(model_, data_);
// Poses
- setSignal (feature_.oMjb, MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
+ setSignal(feature_.oMjb,
+ MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
if (relative_) {
- setSignal (feature_.oMja, MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
+ setSignal(feature_.oMja,
+ MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
}
// Jacobians
- Matrix J (6, model_.nv);
+ Matrix J(6, model_.nv);
J.setZero();
- pinocchio::getJointJacobian (model_, data_, jb_, pinocchio::LOCAL, J);
- setSignal (feature_.jbJjb, J);
+ pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
+ setSignal(feature_.jbJjb, J);
if (relative_) {
J.setZero();
- pinocchio::getJointJacobian (model_, data_, ja_, pinocchio::LOCAL, J);
- setSignal (feature_.jaJja, J);
+ pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
+ setSignal(feature_.jaJja, J);
}
// Desired
- setSignal (feature_.faMfbDes, randomM());
- setSignal (feature_.faNufafbDes, Vector::Random(6));
+ setSignal(feature_.faMfbDes, randomM());
+ setSignal(feature_.faNufafbDes, Vector::Random(6));
double gain = 0;
- //if (time_ % 5 != 0)
- //gain = 2 * (double)rand() / RAND_MAX;
+ // if (time_ % 5 != 0)
+ // gain = 2 * (double)rand() / RAND_MAX;
if (time_ % 2 != 0)
gain = 1;
setGain(gain);
}
- void printInputs()
- {
+ void printInputs() {
BOOST_TEST_MESSAGE("----- inputs -----");
- //BOOST_TEST_MESSAGE("feature_.errorSIN: " << feature_.errorSIN(time_).transpose());
+ // BOOST_TEST_MESSAGE("feature_.errorSIN: " <<
+ // feature_.errorSIN(time_).transpose());
BOOST_TEST_MESSAGE("task.controlGain: " << task_.controlGainSIN(time_));
}
- void recompute()
- {
- FeatureTestBase::recompute ();
+ void recompute() {
+ FeatureTestBase::recompute();
- const SE3
- oMfb = data_.oMf[fb_],
- oMfa = data_.oMf[fa_],
- faMfb (feature_.faMfb.accessCopy().matrix()),
- faMfbDes (feature_.faMfbDes.accessCopy().matrix());
- const Vector& nu (feature_.faNufafbDes.accessCopy());
+ const SE3 oMfb = data_.oMf[fb_], oMfa = data_.oMf[fa_],
+ faMfb(feature_.faMfb.accessCopy().matrix()),
+ faMfbDes(feature_.faMfbDes.accessCopy().matrix());
+ const Vector &nu(feature_.faNufafbDes.accessCopy());
- computeExpectedTaskOutput (
- toVector(oMfa.inverse() * oMfb),
- toVector(faMfbDes),
+ computeExpectedTaskOutput(
+ toVector(oMfa.inverse() * oMfb), toVector(faMfbDes),
(boost::is_same<LieGroup_t, SE3_t>::value
- ? faMfbDes.toActionMatrixInverse() * nu
- : (Vector6d () << nu.head<3>() - faMfb.translation().cross(nu.tail<3>()), faMfbDes.rotation().transpose() * nu.tail<3>()).finished()),
+ ? faMfbDes.toActionMatrixInverse() * nu
+ : (Vector6d() << nu.head<3>() -
+ faMfb.translation().cross(nu.tail<3>()),
+ faMfbDes.rotation().transpose() * nu.tail<3>())
+ .finished()),
LieGroup_t());
checkTaskOutput();
}
- void printOutputs()
- {
+ void printOutputs() {
BOOST_TEST_MESSAGE("----- output -----");
BOOST_TEST_MESSAGE("time: " << time_);
- BOOST_TEST_MESSAGE("feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
- BOOST_TEST_MESSAGE("feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
- BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
- //BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
- //BOOST_TEST_MESSAGE("task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_));
- BOOST_TEST_MESSAGE("Expected task output: " << expectedTaskOutput_.transpose());
+ BOOST_TEST_MESSAGE(
+ "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose());
+ BOOST_TEST_MESSAGE(
+ "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose());
+ BOOST_TEST_MESSAGE("task.taskSOUT: " << task_.taskSOUT(time_));
+ // BOOST_TEST_MESSAGE("task.errorSOUT: " << task_.errorSOUT(time_));
+ // BOOST_TEST_MESSAGE("task.errordtSOUT: " <<
+ // task_.errorTimeDerivativeSOUT(time_));
+ BOOST_TEST_MESSAGE(
+ "Expected task output: " << expectedTaskOutput_.transpose());
}
- virtual void checkJacobian ()
- {
+ virtual void checkJacobian() {
time_++;
// We want to check that e (q+dq, t) ~ e(q, t) + de/dq(q,t) dq
setGain(1.);
- Vector q (pinocchio::randomConfiguration (model_));
- pinocchio::framesForwardKinematics (model_, data_, q);
- pinocchio::computeJointJacobians (model_, data_);
+ Vector q(pinocchio::randomConfiguration(model_));
+ pinocchio::framesForwardKinematics(model_, data_, q);
+ pinocchio::computeJointJacobians(model_, data_);
// Poses
- setSignal (feature_.oMjb, MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
+ setSignal(feature_.oMjb,
+ MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
if (relative_) {
- setSignal (feature_.oMja, MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
+ setSignal(feature_.oMja,
+ MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
}
// Jacobians
- Matrix J (6, model_.nv);
+ Matrix J(6, model_.nv);
J.setZero();
- pinocchio::getJointJacobian (model_, data_, jb_, pinocchio::LOCAL, J);
- setSignal (feature_.jbJjb, J);
+ pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
+ setSignal(feature_.jbJjb, J);
if (relative_) {
J.setZero();
- pinocchio::getJointJacobian (model_, data_, ja_, pinocchio::LOCAL, J);
- setSignal (feature_.jaJja, J);
+ pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
+ setSignal(feature_.jaJja, J);
}
// Desired
- setSignal (feature_.faMfbDes, randomM());
+ setSignal(feature_.faMfbDes, randomM());
// Get task jacobian
Vector e_q = task_.errorSOUT.access(time_);
J = task_.jacobianSOUT.access(time_);
- Eigen::IOFormat PyVectorFmt(Eigen::FullPrecision, 0, ", ", ", ", "", "", "[", "]\n");
- Eigen::IOFormat PyMatrixFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]", "[", "]\n");
+ Eigen::IOFormat PyVectorFmt(Eigen::FullPrecision, 0, ", ", ", ", "", "",
+ "[", "]\n");
+ Eigen::IOFormat PyMatrixFmt(Eigen::FullPrecision, 0, ", ", ",\n", "[", "]",
+ "[", "]\n");
- Vector qdot (Vector::Zero(model_.nv));
+ Vector qdot(Vector::Zero(model_.nv));
double eps = 1e-6;
BOOST_TEST_MESSAGE(
- data_.oMi[ja_].toHomogeneousMatrix().format(PyMatrixFmt) <<
- data_.oMi[jb_].toHomogeneousMatrix().format(PyMatrixFmt) <<
- model_.frames[fa_].placement.toHomogeneousMatrix().format(PyMatrixFmt) <<
- model_.frames[fb_].placement.toHomogeneousMatrix().format(PyMatrixFmt) <<
- J.format(PyMatrixFmt)
- );
-
- for (int i = 0; i < model_.nv; ++i)
- {
+ data_.oMi[ja_].toHomogeneousMatrix().format(PyMatrixFmt)
+ << data_.oMi[jb_].toHomogeneousMatrix().format(PyMatrixFmt)
+ << model_.frames[fa_].placement.toHomogeneousMatrix().format(
+ PyMatrixFmt)
+ << model_.frames[fb_].placement.toHomogeneousMatrix().format(
+ PyMatrixFmt)
+ << J.format(PyMatrixFmt));
+
+ for (int i = 0; i < model_.nv; ++i) {
time_++;
qdot(i) = eps;
// Update pose signals
- Vector q_qdot = pinocchio::integrate (model_, q, qdot);
- pinocchio::framesForwardKinematics (model_, data_, q_qdot);
- setSignal (feature_.oMjb, MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
+ Vector q_qdot = pinocchio::integrate(model_, q, qdot);
+ pinocchio::framesForwardKinematics(model_, data_, q_qdot);
+ setSignal(feature_.oMjb,
+ MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
if (relative_) {
- setSignal (feature_.oMja, MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
+ setSignal(feature_.oMja,
+ MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
}
// Recompute output and check finite diff
Vector e_q_dq = task_.errorSOUT.access(time_);
- BOOST_CHECK_MESSAGE ((((e_q_dq - e_q)/eps) - J.col(i)).maxCoeff() < 1e-4,
+ BOOST_CHECK_MESSAGE(
+ (((e_q_dq - e_q) / eps) - J.col(i)).maxCoeff() < 1e-4,
"Jacobian col " << i << " does not match finite difference.\n"
- << ((e_q_dq - e_q)/eps).format (PyVectorFmt) << '\n'
- << J.col(i).format (PyVectorFmt) << '\n'
- );
+ << ((e_q_dq - e_q) / eps).format(PyVectorFmt) << '\n'
+ << J.col(i).format(PyVectorFmt) << '\n');
qdot(i) = 0.;
}
time_++;
}
- virtual void checkFeedForward ()
- {
+ virtual void checkFeedForward() {
setGain(0.);
// random config
// set inputs
@@ -546,60 +524,60 @@ class TestFeaturePose : public FeatureTestBase
// check that faMfb (q+eps*qdot) ~= faMfb(q) + eps * faNufafbDes
time_++;
- Vector q (pinocchio::randomConfiguration (model_));
- pinocchio::framesForwardKinematics (model_, data_, q);
- pinocchio::computeJointJacobians (model_, data_);
+ Vector q(pinocchio::randomConfiguration(model_));
+ pinocchio::framesForwardKinematics(model_, data_, q);
+ pinocchio::computeJointJacobians(model_, data_);
SE3 faMfb = data_.oMf[fa_].actInv(data_.oMf[fb_]);
// Poses
- setSignal (feature_.oMjb, MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
+ setSignal(feature_.oMjb,
+ MatrixHomogeneous(data_.oMi[jb_].toHomogeneousMatrix()));
if (relative_) {
- setSignal (feature_.oMja, MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
+ setSignal(feature_.oMja,
+ MatrixHomogeneous(data_.oMi[ja_].toHomogeneousMatrix()));
}
// Jacobians
- Matrix J (6, model_.nv);
+ Matrix J(6, model_.nv);
J.setZero();
- pinocchio::getJointJacobian (model_, data_, jb_, pinocchio::LOCAL, J);
- setSignal (feature_.jbJjb, J);
+ pinocchio::getJointJacobian(model_, data_, jb_, pinocchio::LOCAL, J);
+ setSignal(feature_.jbJjb, J);
if (relative_) {
J.setZero();
- pinocchio::getJointJacobian (model_, data_, ja_, pinocchio::LOCAL, J);
- setSignal (feature_.jaJja, J);
+ pinocchio::getJointJacobian(model_, data_, ja_, pinocchio::LOCAL, J);
+ setSignal(feature_.jaJja, J);
}
Matrix faJfafb;
J.setZero();
- pinocchio::getFrameJacobian (model_, data_, fb_, pinocchio::LOCAL, J);
+ pinocchio::getFrameJacobian(model_, data_, fb_, pinocchio::LOCAL, J);
faJfafb = faMfb.toActionMatrix() * J;
J.setZero();
- pinocchio::getFrameJacobian (model_, data_, fa_, pinocchio::LOCAL, J);
+ pinocchio::getFrameJacobian(model_, data_, fa_, pinocchio::LOCAL, J);
faJfafb -= J;
// Desired
- setSignal (feature_.faMfbDes, randomM());
+ setSignal(feature_.faMfbDes, randomM());
// Get task jacobian
task_.jacobianSOUT.recompute(time_);
J = task_.jacobianSOUT.accessCopy();
- Eigen::JacobiSVD<Matrix> svd (J, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ Eigen::JacobiSVD<Matrix> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
- Vector faNufafbDes (Vector::Zero(6));
+ Vector faNufafbDes(Vector::Zero(6));
double eps = 1e-6;
- for (int i = 0; i < 6; ++i)
- {
+ for (int i = 0; i < 6; ++i) {
time_++;
faNufafbDes(i) = 1.;
- setSignal (feature_.faNufafbDes, faNufafbDes);
+ setSignal(feature_.faNufafbDes, faNufafbDes);
task_.taskSOUT.recompute(time_);
- Vector qdot = svd.solve(toVector (task_.taskSOUT.accessCopy()));
+ Vector qdot = svd.solve(toVector(task_.taskSOUT.accessCopy()));
Vector faNufafb = faJfafb * qdot;
EIGEN_VECTOR_IS_APPROX(faNufafbDes, faNufafb, eps);
-
faNufafbDes(i) = 0.;
}
time_++;
@@ -607,9 +585,8 @@ class TestFeaturePose : public FeatureTestBase
};
template <typename TestClass>
-void runTest (TestClass& runner,
- int N = 2)
- //int N = 10)
+void runTest(TestClass &runner, int N = 2)
+// int N = 10)
{
for (int i = 0; i < N; ++i)
runner.checkValue();
@@ -622,51 +599,45 @@ void runTest (TestClass& runner,
BOOST_AUTO_TEST_SUITE(feature_pose)
template <Representation_t representation>
-void feature_pose_absolute_tpl(const std::string& repr)
-{
+void feature_pose_absolute_tpl(const std::string &repr) {
BOOST_TEST_MESSAGE("absolute " << repr);
- TestFeaturePose<representation> testAbsolute(false,"abs"+repr);
+ TestFeaturePose<representation> testAbsolute(false, "abs" + repr);
testAbsolute.setJointFrame();
- runTest (testAbsolute);
+ runTest(testAbsolute);
testAbsolute.setRandomFrame();
- runTest (testAbsolute);
+ runTest(testAbsolute);
}
-BOOST_AUTO_TEST_SUITE (absolute)
+BOOST_AUTO_TEST_SUITE(absolute)
-BOOST_AUTO_TEST_CASE(r3xso3)
-{
+BOOST_AUTO_TEST_CASE(r3xso3) {
feature_pose_absolute_tpl<R3xSO3Representation>("R3xSO3");
}
-BOOST_AUTO_TEST_CASE(se3)
-{
+BOOST_AUTO_TEST_CASE(se3) {
feature_pose_absolute_tpl<SE3Representation>("SE3");
}
BOOST_AUTO_TEST_SUITE_END() // absolute
template <Representation_t representation>
-void feature_pose_relative_tpl(const std::string& repr)
-{
+void feature_pose_relative_tpl(const std::string &repr) {
BOOST_TEST_MESSAGE("relative " << repr);
- TestFeaturePose<representation> testRelative(true ,"rel"+repr);
- runTest (testRelative);
+ TestFeaturePose<representation> testRelative(true, "rel" + repr);
+ runTest(testRelative);
testRelative.setRandomFrame();
- runTest (testRelative);
+ runTest(testRelative);
}
BOOST_AUTO_TEST_SUITE(relative)
-BOOST_AUTO_TEST_CASE (r3xso3)
-{
+BOOST_AUTO_TEST_CASE(r3xso3) {
feature_pose_relative_tpl<R3xSO3Representation>("R3xSO3");
}
-BOOST_AUTO_TEST_CASE (se3)
-{
+BOOST_AUTO_TEST_CASE(se3) {
feature_pose_relative_tpl<SE3Representation>("SE3");
}
diff --git a/unitTesting/features/test_feature_point6d.cpp b/unitTesting/features/test_feature_point6d.cpp
index 291ff956..b58aed73 100644
--- a/unitTesting/features/test_feature_point6d.cpp
+++ b/unitTesting/features/test_feature_point6d.cpp
@@ -12,53 +12,47 @@
/* -------------------------------------------------------------------------- */
#include <iostream>
-#include <sot/core/sot.hh>
-#include <sot/core/feature-point6d.hh>
-#include <sot/core/feature-abstract.hh>
+#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>
-#include <sot/core/task.hh>
+#include <sot/core/feature-abstract.hh>
+#include <sot/core/feature-point6d.hh>
#include <sot/core/gain-adaptive.hh>
-#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/sot.hh>
+#include <sot/core/task.hh>
using namespace std;
using namespace dynamicgraph::sot;
-class TestPoint6d
-{
- public:
- FeaturePoint6d feature_,featureDes_;
+class TestPoint6d {
+public:
+ FeaturePoint6d feature_, featureDes_;
Task task_;
int time_;
- int dim_,robotDim_,featureDim_;
+ int dim_, robotDim_, featureDim_;
dynamicgraph::Vector manual_;
- TestPoint6d(unsigned dim,std::string &name):
- feature_("feature"+name),
- featureDes_("featureDes"+name),
- task_("task"+name),
- time_(0)
+ TestPoint6d(unsigned dim, std::string &name)
+ : feature_("feature" + name), featureDes_("featureDes" + name),
+ task_("task" + name), time_(0)
{
feature_.computationFrame("desired");
feature_.setReference(&featureDes_);
- feature_.selectionSIN=Flags(true);
+ feature_.selectionSIN = Flags(true);
task_.addFeature(feature_);
task_.setWithDerivative(true);
dim_ = dim;
robotDim_ = featureDim_ = dim;
- dynamicgraph::Matrix Jq(dim,dim);
+ dynamicgraph::Matrix Jq(dim, dim);
Jq.setIdentity();
feature_.articularJacobianSIN.setReference(&Jq);
manual_.resize(dim);
}
- void setInputs(MatrixHomogeneous & s,
- MatrixHomogeneous &sd,
- dynamicgraph::Vector &vd,
- double gain)
- {
+ void setInputs(MatrixHomogeneous &s, MatrixHomogeneous &sd,
+ dynamicgraph::Vector &vd, double gain) {
feature_.positionSIN = s;
feature_.positionSIN.access(time_);
feature_.positionSIN.setReady();
@@ -76,21 +70,19 @@ class TestPoint6d
task_.controlGainSIN.setReady();
}
- void printInputs()
- {
+ void printInputs() {
std::cout << "----- inputs -----" << std::endl;
std::cout << "feature_.position: " << feature_.positionSIN(time_)
<< std::endl;
std::cout << "featureDes_.position: " << featureDes_.positionSIN(time_)
<< std::endl;
- std::cout << "featureDes_.velocity: " << featureDes_.velocitySIN(time_).transpose()
- << std::endl;
+ std::cout << "featureDes_.velocity: "
+ << featureDes_.velocitySIN(time_).transpose() << std::endl;
std::cout << "task.controlGain: " << task_.controlGainSIN(time_)
<< std::endl;
}
- int recompute()
- {
+ int recompute() {
feature_.errorSOUT.recompute(time_);
feature_.errordotSOUT.recompute(time_);
@@ -103,70 +95,68 @@ class TestPoint6d
dynamicgraph::Vector vd = featureDes_.velocitySIN;
double gain = task_.controlGainSIN;
dynamicgraph::Vector manual;
- const std::vector<dynamicgraph::sot::MultiBound> & taskTaskSOUT
- =task_.taskSOUT(time_);
-
+ const std::vector<dynamicgraph::sot::MultiBound> &taskTaskSOUT =
+ task_.taskSOUT(time_);
/// Verify the computation of the desired frame.
/// -gain *(s-sd) - ([w]x (sd -s)-vd)
dynamicgraph::Matrix aM;
dynamicgraph::Vector aV;
- aM.resize(3,3);
+ aM.resize(3, 3);
aV.resize(3);
- aM(0,0) = 0.0;aM(0,1) = -vd(5); aM(0,2) = vd(4);
- aM(1,0) = vd(5);aM(1,1) = 0.0; aM(1,2) =-vd(3);
- 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);
-
- aV = aM*aV;
+ aM(0, 0) = 0.0;
+ aM(0, 1) = -vd(5);
+ aM(0, 2) = vd(4);
+ aM(1, 0) = vd(5);
+ aM(1, 1) = 0.0;
+ aM(1, 2) = -vd(3);
+ 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);
+
+ 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;
+ 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;
}
return 0;
}
- void printOutput()
- {
+ void printOutput() {
std::cout << "----- output -----" << std::endl;
std::cout << "time: " << time_ << std::endl;
std::cout << "feature.errorSOUT: " << feature_.errorSOUT(time_).transpose()
<< std::endl;
- std::cout << "feature.errordotSOUT: " << feature_.errordotSOUT(time_).transpose()
- << std::endl;
- std::cout << "task.taskSOUT: " << task_.taskSOUT(time_)
- << std::endl;
+ std::cout << "feature.errordotSOUT: "
+ << feature_.errordotSOUT(time_).transpose() << std::endl;
+ std::cout << "task.taskSOUT: " << task_.taskSOUT(time_) << std::endl;
// std::cout << "task.errorSOUT: " << task_.errorSOUT(time_)
//<< std::endl;
- //std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_)
+ // std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_)
//<< std::endl;
std::cout << "manual: " << manual_.transpose() << std::endl;
}
- int runTest(MatrixHomogeneous &s,
- MatrixHomogeneous &sd,
- dynamicgraph::Vector &vd,
- double gain)
- {
- setInputs(s,sd,vd,gain);
+ int runTest(MatrixHomogeneous &s, MatrixHomogeneous &sd,
+ dynamicgraph::Vector &vd, double gain) {
+ setInputs(s, sd, vd, gain);
printInputs();
- int r= recompute();
+ int r = recompute();
printOutput();
return r;
}
};
-int main( void )
-{
+int main(void) {
// Name of the robot
std::string srobot("Robot");
// Dimension of the robot.
- unsigned int dim=6;
+ unsigned int dim = 6;
// Feature and Desired Feature
MatrixHomogeneous s, sd;
// Desired velocity
@@ -176,55 +166,51 @@ int main( void )
// Result of test
int r;
- TestPoint6d testFeaturePoint6d(dim,srobot);
+ TestPoint6d testFeaturePoint6d(dim, srobot);
std::cout << " ----- Test Velocity -----" << std::endl;
- s .setIdentity();
+ s.setIdentity();
sd.setIdentity();
vd.setConstant(1.);
- gain=0.0;
+ gain = 0.0;
- if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
- {
+ if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
std::cerr << "Failure on 1st test." << std::endl;
return r;
}
std::cout << " ----- Test Position -----" << std::endl;
- s .setIdentity();
+ s.setIdentity();
sd.setIdentity();
sd.translation()[2] = 2.0;
vd.setZero();
- gain=1.0;
+ gain = 1.0;
- if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
- {
+ if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
std::cerr << "Failure on 2nd test." << std::endl;
return r;
}
std::cout << " ----- Test both -----" << std::endl;
- s .setIdentity();
+ s.setIdentity();
sd.setIdentity();
sd.translation()[2] = 2.0;
vd.setConstant(1.);
- gain=3.0;
+ gain = 3.0;
- if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
- {
+ if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
std::cerr << "Failure on 3th test." << std::endl;
return r;
}
std::cout << " ----- Test both again -----" << std::endl;
- s .setIdentity();
+ s.setIdentity();
sd.setIdentity();
sd.translation()[2] = 2.0;
vd.setConstant(1.);
- gain=3.0;
+ gain = 3.0;
- if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
- {
+ if ((r = testFeaturePoint6d.runTest(s, sd, vd, gain)) < 0) {
std::cerr << "Failure on 4th test." << std::endl;
return r;
}
diff --git a/unitTesting/filters/test_filter_differentiator.cpp b/unitTesting/filters/test_filter_differentiator.cpp
index 667a8504..5915b007 100644
--- a/unitTesting/filters/test_filter_differentiator.cpp
+++ b/unitTesting/filters/test_filter_differentiator.cpp
@@ -9,35 +9,33 @@
#include <iostream>
#include <sot/core/debug.hh>
-
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
#include <sot/core/filter-differentiator.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-#define BOOST_TEST_MODULE test-filter-differentiator
+#define BOOST_TEST_MODULE test - filter - differentiator
-#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
+#include <boost/test/unit_test.hpp>
using boost::test_tools::output_test_stream;
-BOOST_AUTO_TEST_CASE(test_filter_differentiator)
-{
- sot::FilterDifferentiator *aFilterDiff = new
- FilterDifferentiator("filter_differentiator");
+BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
+ sot::FilterDifferentiator *aFilterDiff =
+ new FilterDifferentiator("filter_differentiator");
+
+ Eigen::VectorXd filter_num(7), filter_den(7);
- Eigen::VectorXd filter_num(7),filter_den(7);
-
filter_num(0) = 2.16439898e-05;
filter_num(1) = 4.43473520e-05;
filter_num(2) = -1.74065002e-05;
@@ -53,15 +51,15 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator)
filter_den(4) = 9.68705647;
filter_den(5) = -3.52968633;
filter_den(6) = 0.53914042;
-
- double timestep=0.001;
- int xSize=16;
- aFilterDiff->init(timestep,xSize,filter_num,filter_den);
+
+ double timestep = 0.001;
+ int xSize = 16;
+ aFilterDiff->init(timestep, xSize, filter_num, filter_den);
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;
@@ -70,21 +68,19 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator)
aFilterDiff->m_x_filteredSOUT.get(output);
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"
+ "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"));
+ "101.461\n"
+ "71.6195\n"
+ "76.5931\n"
+ "40.7834\n"));
}
-
-
diff --git a/unitTesting/filters/test_madgwick_ahrs.cpp b/unitTesting/filters/test_madgwick_ahrs.cpp
index 44e2d6ed..79592a00 100644
--- a/unitTesting/filters/test_madgwick_ahrs.cpp
+++ b/unitTesting/filters/test_madgwick_ahrs.cpp
@@ -9,44 +9,44 @@
#include <iostream>
#include <sot/core/debug.hh>
-
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
#include <sot/core/madgwickahrs.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-#define BOOST_TEST_MODULE test-filter-differentiator
+#define BOOST_TEST_MODULE test - filter - differentiator
-#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
+#include <boost/test/unit_test.hpp>
using boost::test_tools::output_test_stream;
-BOOST_AUTO_TEST_CASE(test_filter_differentiator)
-{
- sot::MadgwickAHRS *aFilter = new
- MadgwickAHRS("MadgwickAHRS");
+BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
+ sot::MadgwickAHRS *aFilter = new MadgwickAHRS("MadgwickAHRS");
-
- double timestep=0.001,beta=0.01;
+ double timestep = 0.001, beta = 0.01;
aFilter->init(timestep);
aFilter->set_beta(beta);
srand(0);
dynamicgraph::Vector acc(3);
dynamicgraph::Vector angvel(3);
- acc(0)=0.3; acc(1)=0.2; acc(2)=0.3;
+ acc(0) = 0.3;
+ acc(1) = 0.2;
+ acc(2) = 0.3;
aFilter->m_accelerometerSIN = acc;
- angvel(0)=0.1;angvel(1)=-0.1;angvel(2)=0.3;
+ angvel(0) = 0.1;
+ angvel(1) = -0.1;
+ angvel(2) = 0.3;
aFilter->m_gyroscopeSIN = angvel;
aFilter->m_imu_quatSOUT.recompute(0);
output_test_stream output;
@@ -55,9 +55,7 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator)
aFilter->m_imu_quatSOUT.get(anoss);
BOOST_CHECK(output.is_equal(" 1\n"
- " 5.5547e-05\n"
- "-5.83205e-05\n"
- " 0.00015\n"));
+ " 5.5547e-05\n"
+ "-5.83205e-05\n"
+ " 0.00015\n"));
}
-
-
diff --git a/unitTesting/filters/test_madgwick_arhs.cpp b/unitTesting/filters/test_madgwick_arhs.cpp
index 050558fb..5c8ffdf3 100644
--- a/unitTesting/filters/test_madgwick_arhs.cpp
+++ b/unitTesting/filters/test_madgwick_arhs.cpp
@@ -9,44 +9,44 @@
#include <iostream>
#include <sot/core/debug.hh>
-
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
#include <sot/core/madgwickahrs.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-#define BOOST_TEST_MODULE test-filter-differentiator
+#define BOOST_TEST_MODULE test - filter - differentiator
-#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
+#include <boost/test/unit_test.hpp>
using boost::test_tools::output_test_stream;
-BOOST_AUTO_TEST_CASE(test_filter_differentiator)
-{
- sot::MadgwickAHRS *aFilter = new
- MadgwickAHRS("MadgwickAHRS");
+BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
+ sot::MadgwickAHRS *aFilter = new MadgwickAHRS("MadgwickAHRS");
-
- double timestep=0.001,beta=0.01;
+ double timestep = 0.001, beta = 0.01;
aFilter->init(timestep);
aFilter->set_beta(beta);
srand(0);
dynamicgraph::Vector acc(3);
dynamicgraph::Vector angvel(3);
- acc(0)=0.3; acc(1)=0.2; acc(2)=0.3;
+ acc(0) = 0.3;
+ acc(1) = 0.2;
+ acc(2) = 0.3;
aFilter->m_accelerometerSIN = acc;
- angvel(0)=0.1;angvel(1)=-0.1;angvel(2)=0.3;
+ angvel(0) = 0.1;
+ angvel(1) = -0.1;
+ angvel(2) = 0.3;
aFilter->m_gyroscopeSIN = angvel;
aFilter->m_imu_quatSOUT.recompute(0);
output_test_stream output;
@@ -55,21 +55,19 @@ BOOST_AUTO_TEST_CASE(test_filter_differentiator)
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"
+ "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"));
+ "101.461\n"
+ "71.6195\n"
+ "76.5931\n"
+ "40.7834\n"));
}
-
-
diff --git a/unitTesting/math/matrix-homogeneous.cpp b/unitTesting/math/matrix-homogeneous.cpp
index d08a27ba..08e4b99e 100644
--- a/unitTesting/math/matrix-homogeneous.cpp
+++ b/unitTesting/math/matrix-homogeneous.cpp
@@ -4,110 +4,112 @@
#define BOOST_TEST_MODULE matrix_homogeneous
-#include <boost/test/unit_test.hpp>
+#include <boost/math/constants/constants.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/output_test_stream.hpp>
-#include <boost/math/constants/constants.hpp>
+#include <boost/test/unit_test.hpp>
#include <sot/core/matrix-geometry.hh>
-using boost::test_tools::output_test_stream;
using boost::math::constants::pi;
+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) \
- BOOST_REQUIRE_CLOSE(LEFT (i, j), RIGHT (i, j), TOLERANCE)
+#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) \
- MATRIX_BOOST_REQUIRE_CLOSE (4, 4, 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 BOOST_CHECK_SMALL(M(i,j), .01*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 \
+ 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.; M (3, 3) = 1. \
+#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)
-{
- for (unsigned int i=0; i<1000; i++) {
+BOOST_AUTO_TEST_CASE(product) {
+ for (unsigned int i = 0; i < 1000; i++) {
double tx, ty, tz;
double roll, pitch, yaw;
dynamicgraph::sot::MatrixHomogeneous H1;
- tx = (10.*rand())/RAND_MAX - 5.;
- ty = (10.*rand())/RAND_MAX - 5.;
- tz = (10.*rand())/RAND_MAX - 5.;
- roll = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- pitch = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- yaw = (2*pi<double>()*rand())/RAND_MAX - pi<double>();
+ tx = (10. * rand()) / RAND_MAX - 5.;
+ ty = (10. * rand()) / RAND_MAX - 5.;
+ tz = (10. * rand()) / RAND_MAX - 5.;
+ roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
dg::Matrix M1(H1.matrix());
dynamicgraph::sot::MatrixHomogeneous H2;
- tx = (10.*rand())/RAND_MAX;
- ty = (10.*rand())/RAND_MAX - 5.;
- tz = (10.*rand())/RAND_MAX - 5.;
- roll = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- pitch = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- yaw = (2*pi<double>()*rand())/RAND_MAX - pi<double>();
+ tx = (10. * rand()) / RAND_MAX;
+ ty = (10. * rand()) / RAND_MAX - 5.;
+ tz = (10. * rand()) / RAND_MAX - 5.;
+ roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
dg::Matrix M2(H2.matrix());
- dynamicgraph::sot::MatrixHomogeneous H3 = H1*H2;
- dg::Matrix M3; M3 = M1*M2;
+ dynamicgraph::sot::MatrixHomogeneous H3 = H1 * H2;
+ dg::Matrix M3;
+ M3 = M1 * M2;
- MATRIX_4x4_BOOST_REQUIRE_CLOSE (M3, H3.matrix(), 0.0001);
+ MATRIX_4x4_BOOST_REQUIRE_CLOSE(M3, H3.matrix(), 0.0001);
}
}
-BOOST_AUTO_TEST_CASE (inverse)
-{
- for (unsigned int i=0; i<1000; i++) {
+BOOST_AUTO_TEST_CASE(inverse) {
+ for (unsigned int i = 0; i < 1000; i++) {
double tx, ty, tz;
double roll, pitch, yaw;
dynamicgraph::sot::MatrixHomogeneous H1;
- tx = (10.*rand())/RAND_MAX - 5.;
- ty = (10.*rand())/RAND_MAX - 5.;
- tz = (10.*rand())/RAND_MAX - 5.;
- roll = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- pitch = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- yaw = (2*pi<double>()*rand())/RAND_MAX - pi<double>();
+ tx = (10. * rand()) / RAND_MAX - 5.;
+ ty = (10. * rand()) / RAND_MAX - 5.;
+ tz = (10. * rand()) / RAND_MAX - 5.;
+ roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
dynamicgraph::sot::MatrixHomogeneous H2;
- tx = (10.*rand())/RAND_MAX;
- ty = (10.*rand())/RAND_MAX - 5.;
- tz = (10.*rand())/RAND_MAX - 5.;
- roll = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- pitch = (pi<double>()*rand())/RAND_MAX - .5*pi<double>();
- yaw = (2*pi<double>()*rand())/RAND_MAX - pi<double>();
+ tx = (10. * rand()) / RAND_MAX;
+ ty = (10. * rand()) / RAND_MAX - 5.;
+ tz = (10. * rand()) / RAND_MAX - 5.;
+ roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
+ yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
- dynamicgraph::sot::MatrixHomogeneous H3 = H1*H2;
+ dynamicgraph::sot::MatrixHomogeneous H3 = H1 * H2;
dynamicgraph::sot::MatrixHomogeneous invH1, invH2, invH3;
invH1 = H1.inverse();
invH2 = H2.inverse();
invH3 = H3.inverse();
dynamicgraph::sot::MatrixHomogeneous I4;
- dynamicgraph::sot::MatrixHomogeneous P1 = H1*invH1;
- dynamicgraph::sot::MatrixHomogeneous P2 = H2*invH2;
- dynamicgraph::sot::MatrixHomogeneous P3 = H3*invH3;
- dynamicgraph::sot::MatrixHomogeneous P4 = invH2*invH1;
+ dynamicgraph::sot::MatrixHomogeneous P1 = H1 * invH1;
+ dynamicgraph::sot::MatrixHomogeneous P2 = H2 * invH2;
+ dynamicgraph::sot::MatrixHomogeneous P3 = H3 * invH3;
+ dynamicgraph::sot::MatrixHomogeneous P4 = invH2 * invH1;
MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P1, 0.0001);
MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(P2, 0.0001);
diff --git a/unitTesting/math/matrix-twist.cpp b/unitTesting/math/matrix-twist.cpp
index b55fef66..436e9de3 100644
--- a/unitTesting/math/matrix-twist.cpp
+++ b/unitTesting/math/matrix-twist.cpp
@@ -4,51 +4,45 @@
#define BOOST_TEST_MODULE matrix_twist
-#include <boost/test/unit_test.hpp>
#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) \
- BOOST_REQUIRE_CLOSE(LEFT (i, j), RIGHT (i, j), 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; \
- 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, A14, A15, \
- A20, A21, A22, A23, A24, A25, \
- A30, A31, A32, A33, A34, A35, \
- A40, A41, A42, A43, A44, A45, \
- A50, A51, A52, A53, A54, A55) \
- M (0, 0) = A00, M (0, 1) = A01, M (0, 2) = A02, M (0, 3) = A03, \
- M (0, 4) = A04, M (0, 5) = A05; \
- M (1, 0) = A10, M (1, 1) = A11, M (1, 2) = A12, M (1, 3) = A13, \
- M (1, 4) = A14, M (1, 5) = A15; \
- M (2, 0) = A20, M (2, 1) = A21, M (2, 2) = A22, M (2, 3) = A23, \
- M (2, 4) = A24, M (2, 5) = A25; \
- M (3, 0) = A30, M (3, 1) = A31, M (3, 2) = A32, M (3, 3) = A33, \
- M (3, 4) = A34, M (3, 5) = A35; \
- M (4, 0) = A40, M (4, 1) = A41, M (4, 2) = A42, M (4, 3) = A43, \
- M (4, 4) = A44, M (4, 5) = A45; \
- M (5, 0) = A50, M (5, 1) = A51, M (5, 2) = A52, M (5, 3) = A53, \
- M (5, 4) = A54, M (5, 5) = A55
+#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) \
+ 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; \
+ 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, \
+ A14, A15, A20, A21, A22, A23, A24, A25, A30, A31, A32, \
+ A33, A34, A35, A40, A41, A42, A43, A44, A45, A50, A51, \
+ A52, A53, A54, A55) \
+ M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03, M(0, 4) = A04, \
+ M(0, 5) = A05; \
+ M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13, M(1, 4) = A14, \
+ M(1, 5) = A15; \
+ M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23, M(2, 4) = A24, \
+ M(2, 5) = A25; \
+ M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33, M(3, 4) = A34, \
+ M(3, 5) = A35; \
+ M(4, 0) = A40, M(4, 1) = A41, M(4, 2) = A42, M(4, 3) = A43, M(4, 4) = A44, \
+ M(4, 5) = A45; \
+ M(5, 0) = A50, M(5, 1) = A51, M(5, 2) = A52, M(5, 3) = A53, M(5, 4) = A54, \
+ M(5, 5) = A55
// For reminder:
//
@@ -65,163 +59,123 @@ using boost::test_tools::output_test_stream;
// Twist(M)^{-1} = [R^T ; -R^T t^]
// [0 ; R^T ]
-
-BOOST_AUTO_TEST_CASE (constructor_trivial_identity)
-{
+BOOST_AUTO_TEST_CASE(constructor_trivial_identity) {
dynamicgraph::sot::MatrixHomogeneous M(Eigen::Matrix4d::Identity());
- dynamicgraph::sot::MatrixTwist twist; dynamicgraph::sot::buildFrom(M, twist);
+ dynamicgraph::sot::MatrixTwist twist;
+ dynamicgraph::sot::buildFrom(M, twist);
- dynamicgraph::Matrix twistRef (6, 6);
+ 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.;
+ twistRef(i, j) = (i == j) ? 1. : 0.;
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twist, twistRef, 0.001);
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
}
-BOOST_AUTO_TEST_CASE (constructor_rotation_only)
-{
+BOOST_AUTO_TEST_CASE(constructor_rotation_only) {
dynamicgraph::sot::MatrixHomogeneous M;
- MATRIX_4x4_INIT (M,
- 0., 0., 1., 0.,
- 1., 0., 0., 0.,
- 0., -1., 0., 0.,
- 0., 0., 0., 1.);
- dynamicgraph::sot::MatrixTwist twist; dynamicgraph::sot::buildFrom(M, twist);
- dynamicgraph::Matrix twistRef (6, 6);
- MATRIX_6x6_INIT (twistRef,
- 0., 0., 1., 0., 0., 0.,
- 1., 0., 0., 0., 0., 0.,
- 0., -1., 0., 0., 0., 0.,
- 0., 0., 0., 0., 0., 1.,
- 0., 0., 0., 1., 0., 0.,
- 0., 0., 0., 0., -1., 0.);
-
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twist, twistRef, 0.001);
+ MATRIX_4x4_INIT(M, 0., 0., 1., 0., 1., 0., 0., 0., 0., -1., 0., 0., 0., 0.,
+ 0., 1.);
+ dynamicgraph::sot::MatrixTwist twist;
+ dynamicgraph::sot::buildFrom(M, twist);
+ dynamicgraph::Matrix twistRef(6, 6);
+ MATRIX_6x6_INIT(twistRef, 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.,
+ -1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 1.,
+ 0., 0., 0., 0., 0., 0., -1., 0.);
+
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
}
-BOOST_AUTO_TEST_CASE (constructor_translation_only)
-{
+BOOST_AUTO_TEST_CASE(constructor_translation_only) {
dynamicgraph::sot::MatrixHomogeneous M;
double tx = 11.;
double ty = 22.;
double tz = 33.;
- MATRIX_4x4_INIT (M,
- 1., 0., 0., tx,
- 0., 1., 0., ty,
- 0., 0., 1., tz,
- 0., 0., 0., 1.);
- dynamicgraph::sot::MatrixTwist twist; dynamicgraph::sot::buildFrom(M, twist);
-
- dynamicgraph::Matrix twistRef (6, 6);
- MATRIX_6x6_INIT (twistRef,
- 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.);
-
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twist, twistRef, 0.001);
-}
+ MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
+ 1.);
+ dynamicgraph::sot::MatrixTwist twist;
+ dynamicgraph::sot::buildFrom(M, twist);
+
+ dynamicgraph::Matrix twistRef(6, 6);
+ MATRIX_6x6_INIT(twistRef, 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.);
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
+}
-BOOST_AUTO_TEST_CASE (constructor_rotation_translation)
-{
+BOOST_AUTO_TEST_CASE(constructor_rotation_translation) {
dynamicgraph::sot::MatrixHomogeneous M;
double tx = 11.;
double ty = 22.;
double tz = 33.;
- MATRIX_4x4_INIT (M,
- 0., 0., 1., tx,
- 0., -1., 0., ty,
- 1., 0., 0., tz,
- 0., 0., 0., 1.);
- dynamicgraph::sot::MatrixTwist twist; dynamicgraph::sot::buildFrom(M, twist);
-
- dynamicgraph::Matrix twistRef (6, 6);
- MATRIX_6x6_INIT (twistRef,
- 0., 0., 1., ty, tz, 0.,
- 0.,-1., 0., -tx, 0., tz,
- 1., 0., 0., 0., -tx, -ty,
- 0., 0., 0., 0., 0., 1.,
- 0., 0., 0., 0., -1., 0.,
- 0., 0., 0., 1., 0., 0.);
-
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twist, twistRef, 0.001);
-}
+ MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
+ 0., 1.);
+ dynamicgraph::sot::MatrixTwist twist;
+ dynamicgraph::sot::buildFrom(M, twist);
+ dynamicgraph::Matrix twistRef(6, 6);
+ MATRIX_6x6_INIT(twistRef, 0., 0., 1., ty, tz, 0., 0., -1., 0., -tx, 0., tz,
+ 1., 0., 0., 0., -tx, -ty, 0., 0., 0., 0., 0., 1., 0., 0., 0.,
+ 0., -1., 0., 0., 0., 0., 1., 0., 0.);
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
+}
-BOOST_AUTO_TEST_CASE (inverse_translation_only)
-{
+BOOST_AUTO_TEST_CASE(inverse_translation_only) {
dynamicgraph::sot::MatrixHomogeneous M;
double tx = 11.;
double ty = 22.;
double tz = 33.;
- MATRIX_4x4_INIT (M,
- 1., 0., 0., tx,
- 0., 1., 0., ty,
- 0., 0., 1., tz,
- 0., 0., 0., 1.);
- dynamicgraph::sot::MatrixTwist twist; dynamicgraph::sot::buildFrom (M, twist);
+ MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
+ 1.);
+ dynamicgraph::sot::MatrixTwist twist;
+ dynamicgraph::sot::buildFrom(M, twist);
- dynamicgraph::sot::MatrixTwist twistInv(twist.inverse ());
+ dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());
dynamicgraph::sot::MatrixTwist twistInv_;
- twistInv_ = twist.inverse ();
-
- dynamicgraph::Matrix twistRef (6, 6);
- MATRIX_6x6_INIT (twistRef,
- 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.);
-
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twistInv, twistRef, 0.001);
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twistInv_, twistRef, 0.001);
-}
+ twistInv_ = twist.inverse();
+ dynamicgraph::Matrix twistRef(6, 6);
+ MATRIX_6x6_INIT(twistRef, 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.);
-BOOST_AUTO_TEST_CASE (inverse_translation_rotation)
-{
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(inverse_translation_rotation) {
dynamicgraph::sot::MatrixHomogeneous M;
double tx = 11.;
double ty = 22.;
double tz = 33.;
- MATRIX_4x4_INIT (M,
- 0., 0., 1., tx,
- 0., -1., 0., ty,
- 1., 0., 0., tz,
- 0., 0., 0., 1.);
- dynamicgraph::sot::MatrixTwist twist; dynamicgraph::sot::buildFrom(M, twist);
+ MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
+ 0., 1.);
+ dynamicgraph::sot::MatrixTwist twist;
+ dynamicgraph::sot::buildFrom(M, twist);
- dynamicgraph::sot::MatrixTwist twistInv(twist.inverse ());
+ dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());
dynamicgraph::sot::MatrixTwist twistInv_;
- twistInv_ = twist.inverse ();
-
- dynamicgraph::Matrix twistRef (6, 6);
- MATRIX_6x6_INIT (twistRef,
- 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.);
-
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twistInv, twistRef, 0.001);
- MATRIX_6x6_BOOST_REQUIRE_CLOSE (twistInv_, twistRef, 0.001);
+ twistInv_ = twist.inverse();
+
+ dynamicgraph::Matrix twistRef(6, 6);
+ MATRIX_6x6_INIT(twistRef, 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.);
+
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
+ MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
}
diff --git a/unitTesting/matrix/test_operator.cpp b/unitTesting/matrix/test_operator.cpp
index 5d7f925c..964e4bfe 100644
--- a/unitTesting/matrix/test_operator.cpp
+++ b/unitTesting/matrix/test_operator.cpp
@@ -1,21 +1,19 @@
/// Copyright CNRS 2019
/// author: O. Stasse
-#include <iostream>
#include "../../src/matrix/operator.cpp"
+#include <iostream>
-namespace dg= ::dynamicgraph;
+namespace dg = ::dynamicgraph;
using namespace dynamicgraph::sot;
-#define BOOST_TEST_MODULE test-operator
+#define BOOST_TEST_MODULE test - operator
-#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
+#include <boost/test/unit_test.hpp>
using boost::test_tools::output_test_stream;
-
-BOOST_AUTO_TEST_CASE(test_vector_selecter)
-{
+BOOST_AUTO_TEST_CASE(test_vector_selecter) {
// Test VectorSelecter registered as aSelec_of_vector
VectorSelecter aSelec_of_vector;
@@ -28,162 +26,118 @@ 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"));
- dg::Vector vIn(10),vOut(10);
- for(unsigned int i=0;
- i<10;i++)
- vIn(i)=i;
-
- aSelec_of_vector.setBounds(3,5);
- aSelec_of_vector.addBounds(7,10);
- aSelec_of_vector(vIn,vOut);
+ 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;
+
+ aSelec_of_vector.setBounds(3, 5);
+ aSelec_of_vector.addBounds(7, 10);
+ aSelec_of_vector(vIn, vOut);
output << vOut;
- BOOST_CHECK
- (output.is_equal
- ("3\n4\n7\n8\n9"));
+ BOOST_CHECK(output.is_equal("3\n4\n7\n8\n9"));
output << dg::sot::UnaryOp<VectorSelecter>::CLASS_NAME;
- BOOST_CHECK
- (output.is_equal
- ("Selec_of_vector"));
-
- dg::Entity * anEntity = regFunction_Selec_of_vector("test_Selec_of_vector");
- dg::sot::UnaryOp<VectorSelecter> * aVectorSelecter =
- dynamic_cast<dg::sot::UnaryOp<VectorSelecter > * > (anEntity);
+ BOOST_CHECK(output.is_equal("Selec_of_vector"));
+
+ dg::Entity *anEntity = regFunction_Selec_of_vector("test_Selec_of_vector");
+ dg::sot::UnaryOp<VectorSelecter> *aVectorSelecter =
+ dynamic_cast<dg::sot::UnaryOp<VectorSelecter> *>(anEntity);
output << aVectorSelecter->getTypeInName();
- BOOST_CHECK
- (output.is_equal
- ("Vector"));
-
+ BOOST_CHECK(output.is_equal("Vector"));
+
output << aVectorSelecter->getTypeOutName();
- BOOST_CHECK
- (output.is_equal
- ("Vector"));
-
+ BOOST_CHECK(output.is_equal("Vector"));
+
output << aVectorSelecter->getClassName();
- BOOST_CHECK
- (output.is_equal
- ("Selec_of_vector"));
+ 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)
-{
+BOOST_AUTO_TEST_CASE(test_vector_component) {
// Test Vector Component
VectorComponent aComponent_of_vector;
output_test_stream output;
-
+
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);
-
+ 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"));
output << aComponent_of_vector.nameTypeOut();
BOOST_CHECK(output.is_equal("double"));
- dg::Entity * anEntity =
- regFunction_Component_of_vector("test_Component_of_vector");
- dg::sot::UnaryOp<VectorComponent> * aVectorSelecter =
- dynamic_cast<dg::sot::UnaryOp<VectorComponent > * > (anEntity);
+ dg::Entity *anEntity =
+ regFunction_Component_of_vector("test_Component_of_vector");
+ dg::sot::UnaryOp<VectorComponent> *aVectorSelecter =
+ dynamic_cast<dg::sot::UnaryOp<VectorComponent> *>(anEntity);
output << aVectorSelecter->getTypeInName();
- BOOST_CHECK
- (output.is_equal
- ("Vector"));
-
+ BOOST_CHECK(output.is_equal("Vector"));
+
output << aVectorSelecter->getTypeOutName();
- BOOST_CHECK
- (output.is_equal
- ("double"));
-
+ BOOST_CHECK(output.is_equal("double"));
+
output << aVectorSelecter->getClassName();
- BOOST_CHECK
- (output.is_equal
- ("Component_of_vector"));
+ 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)
-{
+BOOST_AUTO_TEST_CASE(test_matrix_selector) {
MatrixSelector aSelec_of_matrix;
output_test_stream output;
-
- aSelec_of_matrix.setBoundsRow(2,4);
- aSelec_of_matrix.setBoundsCol(2,4);
-
- 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;
-
- dg::Matrix resMatrix(2,2);
- aSelec_of_matrix(aMatrix,resMatrix);
- BOOST_CHECK(resMatrix(0,0)==12.0);
- BOOST_CHECK(resMatrix(0,1)==13.0);
- BOOST_CHECK(resMatrix(1,0)==17.0);
- BOOST_CHECK(resMatrix(1,1)==18.0);
-
+
+ aSelec_of_matrix.setBoundsRow(2, 4);
+ aSelec_of_matrix.setBoundsCol(2, 4);
+
+ 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;
+
+ dg::Matrix resMatrix(2, 2);
+ aSelec_of_matrix(aMatrix, resMatrix);
+ BOOST_CHECK(resMatrix(0, 0) == 12.0);
+ BOOST_CHECK(resMatrix(0, 1) == 13.0);
+ BOOST_CHECK(resMatrix(1, 0) == 17.0);
+ BOOST_CHECK(resMatrix(1, 1) == 18.0);
+
output << aSelec_of_matrix.nameTypeIn();
BOOST_CHECK(output.is_equal("Matrix"));
output << aSelec_of_matrix.nameTypeOut();
BOOST_CHECK(output.is_equal("Matrix"));
- dg::Entity * anEntity =
- regFunction_Selec_of_matrix("test_Selec_of_matrix");
- dg::sot::UnaryOp<MatrixSelector> * aMatrixSelector =
- dynamic_cast<dg::sot::UnaryOp<MatrixSelector > * > (anEntity);
+ dg::Entity *anEntity = regFunction_Selec_of_matrix("test_Selec_of_matrix");
+ dg::sot::UnaryOp<MatrixSelector> *aMatrixSelector =
+ dynamic_cast<dg::sot::UnaryOp<MatrixSelector> *>(anEntity);
output << aMatrixSelector->getTypeInName();
- BOOST_CHECK
- (output.is_equal
- ("Matrix"));
-
+ BOOST_CHECK(output.is_equal("Matrix"));
+
output << aMatrixSelector->getTypeOutName();
- BOOST_CHECK
- (output.is_equal
- ("Matrix"));
-
- output << aMatrixSelector->getClassName();
- BOOST_CHECK
- (output.is_equal
- ("Selec_of_matrix"));
+ BOOST_CHECK(output.is_equal("Matrix"));
-
+ output << aMatrixSelector->getClassName();
+ BOOST_CHECK(output.is_equal("Selec_of_matrix"));
}
-
diff --git a/unitTesting/python/CMakeLists.txt b/unitTesting/python/CMakeLists.txt
index d3918ab3..856e58fc 100644
--- a/unitTesting/python/CMakeLists.txt
+++ b/unitTesting/python/CMakeLists.txt
@@ -2,5 +2,5 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
op-point-modifier
)
FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
- ADD_PYTHON_UNIT_TEST("py-${TEST}" "unitTesting/python/${TEST}.py" "python")
+ ADD_PYTHON_UNIT_TEST("py-${TEST}" "unitTesting/python/${TEST}.py")
ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
diff --git a/unitTesting/python/op-point-modifier.py b/unitTesting/python/op-point-modifier.py
index f9f6890f..cfea8cd2 100755
--- a/unitTesting/python/op-point-modifier.py
+++ b/unitTesting/python/op-point-modifier.py
@@ -7,18 +7,18 @@ import unittest
import numpy as np
from dynamic_graph.sot.core import OpPointModifier
-gaze = [(1.0, 0.0, 0.0, 0.025000000000000001), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.64800000000000002),
- (0.0, 0.0, 0.0, 1.0)]
+gaze = tuple(((1.0, 0.0, 0.0, 0.025000000000000001), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.64800000000000002),
+ (0.0, 0.0, 0.0, 1.0)))
-Jgaze = [(1.0, 0.0, 0.0, 0.0, 0.64800000000000002, 0.0),
- (0.0, 1.0, 0.0, -0.64800000000000002, 0.0, 0.025000000000000001),
- (0.0, 0.0, 1.0, 0.0, -0.025000000000000001, 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)]
+Jgaze = tuple(
+ ((1.0, 0.0, 0.0, 0.0, 0.64800000000000002, 0.0), (0.0, 1.0, 0.0, -0.64800000000000002, 0.0, 0.025000000000000001),
+ (0.0, 0.0, 1.0, 0.0, -0.025000000000000001,
+ 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 = [(1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., 0.), (0., 0., 0., 1.)]
+I4 = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., 0.), (0., 0., 0., 1.))
-I6 = [(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.)]
+I6 = ((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.))
class OpPointModifierTest(unittest.TestCase):
diff --git a/unitTesting/signal/test_dep.cpp b/unitTesting/signal/test_dep.cpp
index 38380c15..9d863c26 100644
--- a/unitTesting/signal/test_dep.cpp
+++ b/unitTesting/signal/test_dep.cpp
@@ -11,139 +11,140 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
-#include <iostream>
#include <dynamic-graph/linear-algebra.h>
+#include <iostream>
using namespace std;
using namespace dynamicgraph;
-template< class Res=double >
-class DummyClass
-{
+template <class Res = double> class DummyClass {
public:
- DummyClass( void ) : res(),appel(0),timedata(0) {}
+ DummyClass(void) : res(), appel(0), timedata(0) {}
- Res& fun( Res& res,int t)
- {
- appel++; timedata=t;
+ Res &fun(Res &res, int t) {
+ appel++;
+ timedata = t;
cout << "Inside " << endl;
- for( list< SignalTimeDependent<double,int>* >::iterator it=inputsig.begin();
- it!=inputsig.end();++it )
- { cout << *(*it) << endl; (*it)->access(timedata);}
- for( list< SignalTimeDependent<dynamicgraph::Vector,int>* >::iterator it=inputsigV.begin();
- it!=inputsigV.end();++it )
- { cout << *(*it) << endl; (*it)->access(timedata);}
-
- return res=(*this)();
+ for (list<SignalTimeDependent<double, int> *>::iterator it =
+ inputsig.begin();
+ it != inputsig.end(); ++it) {
+ cout << *(*it) << endl;
+ (*it)->access(timedata);
+ }
+ for (list<SignalTimeDependent<dynamicgraph::Vector, int> *>::iterator it =
+ inputsigV.begin();
+ it != inputsigV.end(); ++it) {
+ cout << *(*it) << endl;
+ (*it)->access(timedata);
+ }
+
+ return res = (*this)();
}
- list< SignalTimeDependent<double,int>* > inputsig;
- list< SignalTimeDependent<dynamicgraph::Vector,int>* > inputsigV;
+ list<SignalTimeDependent<double, int> *> inputsig;
+ list<SignalTimeDependent<dynamicgraph::Vector, int> *> inputsigV;
- void add( SignalTimeDependent<double,int>& sig ){ inputsig.push_back(&sig); }
- void add( SignalTimeDependent<dynamicgraph::Vector,int>& sig ){ inputsigV.push_back(&sig); }
+ void add(SignalTimeDependent<double, int> &sig) { inputsig.push_back(&sig); }
+ void add(SignalTimeDependent<dynamicgraph::Vector, int> &sig) {
+ inputsigV.push_back(&sig);
+ }
- Res operator() ( void );
+ Res operator()(void);
Res res;
int appel;
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)
-{
- res=appel*timedata; return res;
+template <> double DummyClass<double>::operator()(void) {
+ res = appel * timedata;
+ return res;
}
-template<>
-dynamicgraph::Vector DummyClass<dynamicgraph::Vector>::operator() (void)
-{
+template <>
+dynamicgraph::Vector DummyClass<dynamicgraph::Vector>::operator()(void) {
res.resize(3);
- res.fill(appel*timedata); return res;
+ res.fill(appel * timedata);
+ return res;
}
-
// void dispArray( const SignalArray<int> &ar )
// {
// for( unsigned int i=0;i<ar.rank;++i ) cout<<*ar.array[i]<<endl;
// }
#include <vector>
-int main( void )
-{
- DummyClass<double> pro1,pro3,pro5;
- DummyClass<dynamicgraph::Vector> pro2,pro4,pro6;
-
- SignalTimeDependent<double,int> sig5("Sig5");
- SignalTimeDependent<dynamicgraph::Vector,int> sig6("Sig6");
-
- SignalTimeDependent<dynamicgraph::Vector,int> sig4(sig5,"Sig4");
- SignalTimeDependent<dynamicgraph::Vector,int> sig2(sig4<<sig4<<sig4<<sig6,"Sig2");
- SignalTimeDependent<double,int> sig3(sig2<<sig5<<sig6,"Sig3");
- SignalTimeDependent<double,int> sig1( boost::bind(&DummyClass<double>::fun,pro1,_1,_2),
- sig2<<sig3,"Sig1");
-
-// cout << "--- Test Array ------ "<<endl;
-// SignalArray<int> tarr(12);
-// tarr<<sig3<<sig2;//+sig2+sig3;
-// dispArray(sig4<<sig2<<sig3);
-// dispArray(tarr);
-
-
-
-
- //sig1.set( &DummyClass<double>::fun,pro1 );
- sig2.setFunction( boost::bind(&DummyClass<dynamicgraph::Vector>::fun,pro2,_1,_2) );
- sig3.setFunction( boost::bind(&DummyClass<double>::fun,pro3,_1,_2) );
- sig4.setFunction( boost::bind(&DummyClass<dynamicgraph::Vector>::fun,pro4,_1,_2) );
- sig5.setFunction( boost::bind(&DummyClass<double>::fun,pro5,_1,_2) );
- sig6.setFunction( boost::bind(&DummyClass<dynamicgraph::Vector>::fun,pro6,_1,_2) );
-
- // sig1.addDependency(sig2);
- // sig1.addDependency(sig3);
- // sig2.addDependency(sig4);
- // sig2.addDependency(sig4);
- // sig2.addDependency(sig4);
- // sig3.addDependency(sig2);
- // sig4.addDependency(sig5);
- // sig2.addDependency(sig6);
- // sig3.addDependency(sig5);
- // sig3.addDependency(sig6);
-
- pro1.add(sig2);
- pro1.add(sig3);
- pro2.add(sig4);
- pro2.add(sig4);
- pro2.add(sig4);
- pro3.add(sig2);
- pro4.add(sig5);
- pro2.add(sig6);
- pro3.add(sig5);
- pro3.add(sig6);
-
- //sig5.setDependencyType(TimeDependency<int>::ALWAYS_READY);
- //sig6.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
-
- sig6.setReady();
-
- sig1.displayDependencies(cout)<<endl;
-
- cout << "Needs update?"<< endl << sig1.needUpdate(2) << endl;
- sig1.access(2);
- sig1.displayDependencies(cout)<<endl;
- sig2.access(4);
- sig1.displayDependencies(cout)<<endl;
- sig1.access(4);
- sig1.displayDependencies(cout)<<endl;
- sig1.needUpdate(6);
- sig1.needUpdate(6);
+int main(void) {
+ DummyClass<double> pro1, pro3, pro5;
+ DummyClass<dynamicgraph::Vector> pro2, pro4, pro6;
+
+ SignalTimeDependent<double, int> sig5("Sig5");
+ SignalTimeDependent<dynamicgraph::Vector, int> sig6("Sig6");
+
+ SignalTimeDependent<dynamicgraph::Vector, int> sig4(sig5, "Sig4");
+ SignalTimeDependent<dynamicgraph::Vector, int> sig2(
+ sig4 << sig4 << sig4 << sig6, "Sig2");
+ SignalTimeDependent<double, int> sig3(sig2 << sig5 << sig6, "Sig3");
+ SignalTimeDependent<double, int> sig1(
+ boost::bind(&DummyClass<double>::fun, pro1, _1, _2), sig2 << sig3,
+ "Sig1");
+
+ // cout << "--- Test Array ------ "<<endl;
+ // SignalArray<int> tarr(12);
+ // tarr<<sig3<<sig2;//+sig2+sig3;
+ // dispArray(sig4<<sig2<<sig3);
+ // dispArray(tarr);
+
+ // sig1.set( &DummyClass<double>::fun,pro1 );
+ sig2.setFunction(
+ boost::bind(&DummyClass<dynamicgraph::Vector>::fun, pro2, _1, _2));
+ sig3.setFunction(boost::bind(&DummyClass<double>::fun, pro3, _1, _2));
+ sig4.setFunction(
+ boost::bind(&DummyClass<dynamicgraph::Vector>::fun, pro4, _1, _2));
+ sig5.setFunction(boost::bind(&DummyClass<double>::fun, pro5, _1, _2));
+ sig6.setFunction(
+ boost::bind(&DummyClass<dynamicgraph::Vector>::fun, pro6, _1, _2));
+
+ // sig1.addDependency(sig2);
+ // sig1.addDependency(sig3);
+ // sig2.addDependency(sig4);
+ // sig2.addDependency(sig4);
+ // sig2.addDependency(sig4);
+ // sig3.addDependency(sig2);
+ // sig4.addDependency(sig5);
+ // sig2.addDependency(sig6);
+ // sig3.addDependency(sig5);
+ // sig3.addDependency(sig6);
+
+ pro1.add(sig2);
+ pro1.add(sig3);
+ pro2.add(sig4);
+ pro2.add(sig4);
+ pro2.add(sig4);
+ pro3.add(sig2);
+ pro4.add(sig5);
+ pro2.add(sig6);
+ pro3.add(sig5);
+ pro3.add(sig6);
+
+ // sig5.setDependencyType(TimeDependency<int>::ALWAYS_READY);
+ // sig6.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
+
+ sig6.setReady();
+
+ sig1.displayDependencies(cout) << endl;
+
+ cout << "Needs update?" << endl << sig1.needUpdate(2) << endl;
+ sig1.access(2);
+ sig1.displayDependencies(cout) << endl;
+ sig2.access(4);
+ sig1.displayDependencies(cout) << endl;
+ sig1.access(4);
+ sig1.displayDependencies(cout) << endl;
+ sig1.needUpdate(6);
+ sig1.needUpdate(6);
return 0;
-
}
diff --git a/unitTesting/signal/test_depend.cpp b/unitTesting/signal/test_depend.cpp
index 75488278..2d2bef0f 100644
--- a/unitTesting/signal/test_depend.cpp
+++ b/unitTesting/signal/test_depend.cpp
@@ -11,133 +11,130 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
-#include <sot/core/debug.hh>
-#include <iostream>
#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
-{
+template <class Res = double> class DummyClass {
public:
std::string proname;
- list< SignalTimeDependent<double,int>* > inputsig;
- list< SignalTimeDependent<dynamicgraph::Vector,int>* > inputsigV;
+ list<SignalTimeDependent<double, int> *> inputsig;
+ list<SignalTimeDependent<dynamicgraph::Vector, int> *> inputsigV;
public:
- DummyClass( const std::string& n ) : proname(n),res(),appel(0),timedata(0) {}
-
- Res& fun( Res& res,int t)
- {
- appel++; timedata=t;
-
- cout << "Inside " << proname << " -> " << this
- << endl;
- for( list< SignalTimeDependent<double,int>* >::iterator it=inputsig.begin();
- it!=inputsig.end();++it )
- {
- cout << *(*it) << endl;
- (*it)->access(timedata);
- }
- for( list< SignalTimeDependent<dynamicgraph::Vector,int>* >::iterator it=inputsigV.begin();
- it!=inputsigV.end();++it )
- { cout << *(*it) << endl; (*it)->access(timedata);}
-
- return res=(*this)();
+ DummyClass(const std::string &n) : proname(n), res(), appel(0), timedata(0) {}
+
+ Res &fun(Res &res, int t) {
+ appel++;
+ timedata = t;
+
+ cout << "Inside " << proname << " -> " << this << endl;
+ for (list<SignalTimeDependent<double, int> *>::iterator it =
+ inputsig.begin();
+ it != inputsig.end(); ++it) {
+ cout << *(*it) << endl;
+ (*it)->access(timedata);
+ }
+ for (list<SignalTimeDependent<dynamicgraph::Vector, int> *>::iterator it =
+ inputsigV.begin();
+ it != inputsigV.end(); ++it) {
+ cout << *(*it) << endl;
+ (*it)->access(timedata);
+ }
+
+ return res = (*this)();
}
- void add( SignalTimeDependent<double,int>& sig )
- { inputsig.push_back(&sig); }
- void add( SignalTimeDependent<dynamicgraph::Vector,int>& sig )
- { inputsigV.push_back(&sig); }
+ void add(SignalTimeDependent<double, int> &sig) { inputsig.push_back(&sig); }
+ void add(SignalTimeDependent<dynamicgraph::Vector, int> &sig) {
+ inputsigV.push_back(&sig);
+ }
- Res operator() ( void );
+ Res operator()(void);
Res res;
int appel;
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)
-{
- res=appel*timedata; return res;
+template <> double DummyClass<double>::operator()(void) {
+ res = appel * timedata;
+ return res;
}
-template<>
-dynamicgraph::Vector DummyClass<dynamicgraph::Vector>::operator() (void)
-{
+template <>
+dynamicgraph::Vector DummyClass<dynamicgraph::Vector>::operator()(void) {
res.resize(3);
- res.fill(appel*timedata); return res;
+ res.fill(appel * timedata);
+ return res;
}
-
-int main( void )
-{
- DummyClass<double> pro1("pro1"),pro3("pro3"),pro5("pro5");
- DummyClass<dynamicgraph::Vector> pro2("pro2"),pro4("pro4"),pro6("pro6");
-
- SignalTimeDependent<double,int> sig5("Sig5");
- SignalTimeDependent<dynamicgraph::Vector,int> sig6("Sig6");
-
- SignalTimeDependent<dynamicgraph::Vector,int> sig4(sig5,"Sig4");
- SignalTimeDependent<dynamicgraph::Vector,int> sig2(sig4<<sig4<<sig4<<sig6,"Sig2");
- SignalTimeDependent<double,int> sig3(sig2<<sig5<<sig6,"Sig3");
- SignalTimeDependent<double,int> sig1( boost::bind(&DummyClass<double>::fun,&pro1,_1,_2),
- sig2<<sig3,"Sig1");
-
-// cout << "--- Test Array ------ "<<endl;
-// SignalArray<int> tarr(12);
-// tarr<<sig3<<sig2;//+sig2+sig3;
-// dispArray(sig4<<sig2<<sig3);
-// dispArray(tarr);
-
- sig2.setFunction( boost::bind(&DummyClass<dynamicgraph::Vector>::fun,&pro2,_1,_2) );
- sig3.setFunction( boost::bind(&DummyClass<double>::fun,&pro3,_1,_2) );
- sig4.setFunction( boost::bind(&DummyClass<dynamicgraph::Vector>::fun,&pro4,_1,_2) );
- sig5.setFunction( boost::bind(&DummyClass<double>::fun,&pro5,_1,_2) );
- sig6.setFunction( boost::bind(&DummyClass<dynamicgraph::Vector>::fun,&pro6,_1,_2) );
-
- pro1.add(sig2);
- pro1.add(sig3);
- pro2.add(sig4);
- pro2.add(sig4);
- pro2.add(sig4);
- pro3.add(sig2);
- pro4.add(sig5);
- pro2.add(sig6);
- pro3.add(sig5);
- pro3.add(sig6);
-
- sig5.setDependencyType(TimeDependency<int>::ALWAYS_READY);
- sig6.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
-
- sig6.setReady();
-
- sig1.displayDependencies(cout)<<endl;
-
- cout << "Needs update?"<< endl << sig1.needUpdate(2) << endl;
- dgDEBUG(1) << "Access sig1(2) "<<endl;
- sig1.access(2);
- sig1.displayDependencies(cout)<<endl;
- dgDEBUG(1) << "Access sig2(4) "<<endl;
- sig2.access(4);
- sig1.displayDependencies(cout)<<endl;
- dgDEBUG(1) << "Access sig1(4) "<<endl;
- sig1.access(4);
- sig1.displayDependencies(cout)<<endl;
-
- sig1.needUpdate(6);
- sig1.needUpdate(6);
+int main(void) {
+ DummyClass<double> pro1("pro1"), pro3("pro3"), pro5("pro5");
+ DummyClass<dynamicgraph::Vector> pro2("pro2"), pro4("pro4"), pro6("pro6");
+
+ SignalTimeDependent<double, int> sig5("Sig5");
+ SignalTimeDependent<dynamicgraph::Vector, int> sig6("Sig6");
+
+ SignalTimeDependent<dynamicgraph::Vector, int> sig4(sig5, "Sig4");
+ SignalTimeDependent<dynamicgraph::Vector, int> sig2(
+ sig4 << sig4 << sig4 << sig6, "Sig2");
+ SignalTimeDependent<double, int> sig3(sig2 << sig5 << sig6, "Sig3");
+ SignalTimeDependent<double, int> sig1(
+ boost::bind(&DummyClass<double>::fun, &pro1, _1, _2), sig2 << sig3,
+ "Sig1");
+
+ // cout << "--- Test Array ------ "<<endl;
+ // SignalArray<int> tarr(12);
+ // tarr<<sig3<<sig2;//+sig2+sig3;
+ // dispArray(sig4<<sig2<<sig3);
+ // dispArray(tarr);
+
+ sig2.setFunction(
+ boost::bind(&DummyClass<dynamicgraph::Vector>::fun, &pro2, _1, _2));
+ sig3.setFunction(boost::bind(&DummyClass<double>::fun, &pro3, _1, _2));
+ sig4.setFunction(
+ boost::bind(&DummyClass<dynamicgraph::Vector>::fun, &pro4, _1, _2));
+ sig5.setFunction(boost::bind(&DummyClass<double>::fun, &pro5, _1, _2));
+ sig6.setFunction(
+ boost::bind(&DummyClass<dynamicgraph::Vector>::fun, &pro6, _1, _2));
+
+ pro1.add(sig2);
+ pro1.add(sig3);
+ pro2.add(sig4);
+ pro2.add(sig4);
+ pro2.add(sig4);
+ pro3.add(sig2);
+ pro4.add(sig5);
+ pro2.add(sig6);
+ pro3.add(sig5);
+ pro3.add(sig6);
+
+ sig5.setDependencyType(TimeDependency<int>::ALWAYS_READY);
+ sig6.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
+
+ sig6.setReady();
+
+ sig1.displayDependencies(cout) << endl;
+
+ cout << "Needs update?" << endl << sig1.needUpdate(2) << endl;
+ dgDEBUG(1) << "Access sig1(2) " << endl;
+ sig1.access(2);
+ sig1.displayDependencies(cout) << endl;
+ dgDEBUG(1) << "Access sig2(4) " << endl;
+ sig2.access(4);
+ sig1.displayDependencies(cout) << endl;
+ dgDEBUG(1) << "Access sig1(4) " << endl;
+ sig1.access(4);
+ sig1.displayDependencies(cout) << endl;
+
+ sig1.needUpdate(6);
+ sig1.needUpdate(6);
return 0;
-
}
diff --git a/unitTesting/signal/test_ptr.cpp b/unitTesting/signal/test_ptr.cpp
index 7d359178..a6a17cd7 100644
--- a/unitTesting/signal/test_ptr.cpp
+++ b/unitTesting/signal/test_ptr.cpp
@@ -10,104 +10,103 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <sot/core/debug.hh>
-#include <sot/core/exception-abstract.hh>
#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>
#include <sot/core/matrix-geometry.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-template< class Res=double >
-class DummyClass
-{
+template <class Res = double> class DummyClass {
public:
- DummyClass( void ) : res(),appel(0),timedata(0) {}
-
- Res& fun( Res& res,int t)
- {
- appel++; timedata=t;
-
- sotDEBUG(5) << "Inside " << typeid(Res).name() <<endl;
- for( list< SignalTimeDependent<double,int>* >::iterator it=inputsig.begin();
- it!=inputsig.end();++it )
- { sotDEBUG(5) << *(*it) << endl; (*it)->access(timedata);}
- for( list< SignalTimeDependent<dynamicgraph::Vector,int>* >::iterator it=inputsigV.begin();
- it!=inputsigV.end();++it )
- { sotDEBUG(5) << *(*it) << endl; (*it)->access(timedata);}
-
- return res=(*this)();
+ DummyClass(void) : res(), appel(0), timedata(0) {}
+
+ Res &fun(Res &res, int t) {
+ appel++;
+ timedata = t;
+
+ sotDEBUG(5) << "Inside " << typeid(Res).name() << endl;
+ for (list<SignalTimeDependent<double, int> *>::iterator it =
+ inputsig.begin();
+ it != inputsig.end(); ++it) {
+ sotDEBUG(5) << *(*it) << endl;
+ (*it)->access(timedata);
+ }
+ for (list<SignalTimeDependent<dynamicgraph::Vector, int> *>::iterator it =
+ inputsigV.begin();
+ it != inputsigV.end(); ++it) {
+ sotDEBUG(5) << *(*it) << endl;
+ (*it)->access(timedata);
+ }
+
+ return res = (*this)();
}
- list< SignalTimeDependent<double,int>* > inputsig;
- list< SignalTimeDependent<dynamicgraph::Vector,int>* > inputsigV;
+ list<SignalTimeDependent<double, int> *> inputsig;
+ list<SignalTimeDependent<dynamicgraph::Vector, int> *> inputsigV;
- void add( SignalTimeDependent<double,int>& sig ){ inputsig.push_back(&sig); }
- void add( SignalTimeDependent<dynamicgraph::Vector,int>& sig ){ inputsigV.push_back(&sig); }
+ void add(SignalTimeDependent<double, int> &sig) { inputsig.push_back(&sig); }
+ void add(SignalTimeDependent<dynamicgraph::Vector, int> &sig) {
+ inputsigV.push_back(&sig);
+ }
- Res operator() ( void );
+ Res operator()(void);
Res res;
int appel;
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)
-{
- res=appel*timedata; return res;
+template <> double DummyClass<double>::operator()(void) {
+ res = appel * timedata;
+ return res;
}
-template<>
-dynamicgraph::Vector DummyClass<dynamicgraph::Vector>::operator() (void)
-{
+template <>
+dynamicgraph::Vector DummyClass<dynamicgraph::Vector>::operator()(void) {
res.resize(3);
- res.fill(appel*timedata); return res;
+ 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;
}
-
// void dispArray( const SignalArray<int> &ar )
// {
// for( unsigned int i=0;i<ar.rank;++i ) sotDEBUG(5)<<*ar.array[i]<<endl;
// }
-void funtest( dynamicgraph::Vector& /*v*/ ){ }
+void funtest(dynamicgraph::Vector & /*v*/) {}
#include <vector>
-int main( void )
-{
- DummyClass<VectorUTheta> pro3;
- SignalTimeDependent<VectorUTheta,int> sig3(sotNOSIGNAL,"Sig3");
- SignalPtr<Eigen::AngleAxisd,int> sigTo3( NULL,"SigTo3" );
- dynamicgraph::Vector v;
- VectorUTheta v3;
- funtest(v);
-
- sig3.setFunction( boost::bind(&DummyClass<VectorUTheta>::fun,pro3,_1,_2) );
- try
- {
- sigTo3.plug(&sig3);
- }
- catch( sot::ExceptionAbstract& e ) { cout << "Plugin error "<<e << endl; exit(1); }
- sig3.access(1); sig3.setReady();
- sigTo3.access(2);
- cout << sigTo3.access(2);
+int main(void) {
+ DummyClass<VectorUTheta> pro3;
+ SignalTimeDependent<VectorUTheta, int> sig3(sotNOSIGNAL, "Sig3");
+ SignalPtr<Eigen::AngleAxisd, int> sigTo3(NULL, "SigTo3");
+ dynamicgraph::Vector v;
+ VectorUTheta v3;
+ funtest(v);
+
+ sig3.setFunction(boost::bind(&DummyClass<VectorUTheta>::fun, pro3, _1, _2));
+ try {
+ sigTo3.plug(&sig3);
+ } catch (sot::ExceptionAbstract &e) {
+ cout << "Plugin error " << e << endl;
+ exit(1);
+ }
+ sig3.access(1);
+ sig3.setReady();
+ sigTo3.access(2);
+ cout << sigTo3.access(2);
return 0;
-
}
diff --git a/unitTesting/signal/test_ptrcast.cpp b/unitTesting/signal/test_ptrcast.cpp
index 34e8a995..e64ef95c 100644
--- a/unitTesting/signal/test_ptrcast.cpp
+++ b/unitTesting/signal/test_ptrcast.cpp
@@ -11,22 +11,20 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
-#include <sot/core/matrix-geometry.hh>
#include <dynamic-graph/linear-algebra.h>
#include <iostream>
+#include <sot/core/matrix-geometry.hh>
using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
+Signal<dynamicgraph::Matrix, int> base("base");
+Signal<dynamicgraph::Matrix, int> sig("matrix");
+SignalPtr<dynamicgraph::Matrix, int> sigptr(&base);
-Signal<dynamicgraph::Matrix,int> base("base");
-Signal<dynamicgraph::Matrix,int> sig("matrix");
-SignalPtr<dynamicgraph::Matrix,int> sigptr(&base);
-
-Signal<MatrixRotation,int> sigMR("matrixRot");
+Signal<MatrixRotation, int> sigMR("matrixRot");
-int main( void )
-{
+int main(void) {
sigptr.plug(&sig);
cout << "Correctly plugged matrix" << endl;
// sigptr.plug(&sigMR);
diff --git a/unitTesting/signal/test_signal.cpp b/unitTesting/signal/test_signal.cpp
index 883beb28..5ef7c856 100644
--- a/unitTesting/signal/test_signal.cpp
+++ b/unitTesting/signal/test_signal.cpp
@@ -11,63 +11,60 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic-graph/all-signals.h>
-#include <iostream>
#include <dynamic-graph/linear-algebra.h>
+#include <iostream>
using namespace std;
using namespace dynamicgraph;
-
-
-class DummyClass
-{
+class DummyClass {
public:
- dynamicgraph::Vector& fun( dynamicgraph::Vector& res,double j )
- { res.resize(3); res.fill(j); return res; }
-
-
+ dynamicgraph::Vector &fun(dynamicgraph::Vector &res, double j) {
+ res.resize(3);
+ res.fill(j);
+ return res;
+ }
};
dynamicgraph::Vector data(6);
-Signal<dynamicgraph::Vector,double> sig("sigtest");
+Signal<dynamicgraph::Vector, double> sig("sigtest");
DummyClass dummy;
-dynamicgraph::Vector& fun( dynamicgraph::Vector& res,double /*j*/ ) { return res=data; }
+dynamicgraph::Vector &fun(dynamicgraph::Vector &res, double /*j*/) {
+ return res = data;
+}
-int main( void )
-{
+int main(void) {
data.fill(1);
- cout << "data: " << data <<endl;
-
- sig.setConstant( data );
- cout << "Constant: " << sig.access(1.) <<endl;
- data*=2;
- cout << "Constant: " << sig(1.) <<endl;
-
- sig.setReference( &data );
- cout << "Reference: " << sig(1.) <<endl;
- data*=2;
- cout << "Reference: " << sig(1.) <<endl;
-
- sig.setFunction( &fun );
- cout << "Function: " << sig(1.) <<endl;
- data*=2;
- cout << "Function: " << sig(1.) <<endl;
-
-
- //boost::function2<int,int,double> onClick = (&DummyClass::fun, &dummy, _1,_2) ;
- //boost::function<> onClick = boost::bind(&DummyClass::fun, &dummy);
- sig.setFunction( boost::bind(&DummyClass::fun, &dummy, _1,_2) );
- cout << "Function: " << sig(1.5) <<endl;
- data*=2;
- cout << "Function: " << sig(1.34) <<endl;
-
-
-// sig.setFunction(&DummyClass::fun, dummy);
-// cout << "Function: " << sig(1.5) <<endl;
-// data*=2;
-// cout << "Function: " << sig(12.34) <<endl;
-
+ cout << "data: " << data << endl;
+
+ sig.setConstant(data);
+ cout << "Constant: " << sig.access(1.) << endl;
+ data *= 2;
+ cout << "Constant: " << sig(1.) << endl;
+
+ sig.setReference(&data);
+ cout << "Reference: " << sig(1.) << endl;
+ data *= 2;
+ cout << "Reference: " << sig(1.) << endl;
+
+ sig.setFunction(&fun);
+ cout << "Function: " << sig(1.) << endl;
+ data *= 2;
+ cout << "Function: " << sig(1.) << endl;
+
+ // boost::function2<int,int,double> onClick = (&DummyClass::fun, &dummy,
+ // _1,_2) ; boost::function<> onClick = boost::bind(&DummyClass::fun,
+ // &dummy);
+ sig.setFunction(boost::bind(&DummyClass::fun, &dummy, _1, _2));
+ cout << "Function: " << sig(1.5) << endl;
+ data *= 2;
+ cout << "Function: " << sig(1.34) << endl;
+
+ // sig.setFunction(&DummyClass::fun, dummy);
+ // cout << "Function: " << sig(1.5) <<endl;
+ // data*=2;
+ // cout << "Function: " << sig(12.34) <<endl;
return 0;
}
diff --git a/unitTesting/sot/test_solverSoth.cpp b/unitTesting/sot/test_solverSoth.cpp
index ebbeca28..5f9dd228 100644
--- a/unitTesting/sot/test_solverSoth.cpp
+++ b/unitTesting/sot/test_solverSoth.cpp
@@ -8,15 +8,14 @@
*/
#define VP_DEBUG_MODE 45
+#include <fstream>
#include <sot/core/debug.hh>
#include <sot/core/solver-hierarchical-inequalities.hh>
-#include <fstream>
-
#ifndef WIN32
-# include <sys/time.h>
+#include <sys/time.h>
#else /*WIN32*/
-# include <sot/core/utils-windows.hh>
+#include <sot/core/utils-windows.hh>
#endif /*WIN32*/
//#define WITH_CHRONO
@@ -26,10 +25,9 @@ using namespace dynamicgraph::sot;
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-void parseTest( const std::string filename )
-{
+void parseTest(const std::string filename) {
using namespace std;
- std::ifstream off( filename.c_str() );
+ std::ifstream off(filename.c_str());
std::string bs;
bubMatrix Rh;
@@ -42,238 +40,277 @@ void parseTest( const std::string filename )
std::vector<ConstraintMem::BoundSideVector> bounds;
int nJ;
- int me,mi;
- bubMatrix Je,Ji;
- bubVector ee,eiInf,eiSup;
+ int me, mi;
+ bubMatrix Je, Ji;
+ bubVector ee, eiInf, eiSup;
ConstraintMem::BoundSideVector eiBoundSide;
- off >> bs; if(bs!="variable") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> bs; if(bs!="size") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> bs;
+ if (bs != "variable") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
+ off >> bs;
+ if (bs != "size") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
off >> nJ;
sotRotationComposedInExtenso Qh(nJ);
- std::deque<SolverHierarchicalInequalities*> solvers;
-
- for( unsigned int level=0;;++level )
- {
- /* --- Parse egalities --- */
- off >> bs;
- if(bs=="end") { break; }
- else if(bs!="level") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> bs; if(bs!="equalities") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> me;
- Je.resize(me,nJ); ee.resize(me);
- if(me>0)
- for( int i=0;i<me;++i )
- {
- for( int j=0;j<nJ;++j )
- off >> Je(i,j);
- off >> ee(i);
- }
-
- /* --- Parse inequalities --- */
- off >> bs; if(bs!="inequalities") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> mi;
- Ji.resize(mi,nJ); eiInf.resize(mi); eiSup.resize(mi); eiBoundSide.resize(mi);
- if(mi>0)
- for( int i=0;i<mi;++i )
- {
- for( int j=0;j<nJ;++j )
- off >> Ji(i,j);
- std::string number;
- eiBoundSide[i] = ConstraintMem::BOUND_VOID;
- off>>number;
- if( number!="X" )
- {
- eiBoundSide[i] = (ConstraintMem::BoundSideType)
- (eiBoundSide[i]|ConstraintMem::BOUND_INF);
- eiInf(i) = atof(number.c_str());
- } else { eiInf(i) = 1e-66; }
- off>>number;
- if( number!="X" )
- {
- eiBoundSide[i] = (ConstraintMem::BoundSideType)
- (eiBoundSide[i]|ConstraintMem::BOUND_SUP);
- eiSup(i) = atof(number.c_str());
- } else { eiSup(i) = 1e-66; }
- }
-
- struct timeval t0,t1;
- double dtsolver;
- sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
- sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
- sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
- sotDEBUG(5) << "ee" << level << " = " << (MATLAB) ee << endl;
- sotDEBUG(5) << "eiInf" << level << " = " << (MATLAB) eiInf << endl;
- sotDEBUG(5) << "eiSup" << level << " = " << (MATLAB) eiSup << endl;
- sotDEBUG(5) << "Je" << level << " = " << (MATLAB) Je << endl;
- sotDEBUG(5) << "Ji" << level << " = " << (MATLAB) Ji << endl;
- gettimeofday(&t0,NULL);
-
-
- Jes.push_back(Je);
- Jis.push_back(Ji);
- ees.push_back(ee);
- eiInfs.push_back(eiInf);
- eiSups.push_back(eiSup);
- bounds.push_back(eiBoundSide);
-
- sotDEBUG(1) << "--- Level " << level << std::endl;
- SolverHierarchicalInequalities * solver
- = new SolverHierarchicalInequalities(nJ,Qh,Rh,constraintH);
- solver->initConstraintSize(Je.size1()+Ji.size1());
- if( solvers.size()==0 ) solver->setInitialConditionVoid();
- else
- {
- SolverHierarchicalInequalities * solverPrec
- = solvers.back();
- solver->setInitialCondition( solverPrec->u0,solverPrec->rankh );
+ std::deque<SolverHierarchicalInequalities *> solvers;
+
+ for (unsigned int level = 0;; ++level) {
+ /* --- Parse egalities --- */
+ off >> bs;
+ if (bs == "end") {
+ break;
+ } else if (bs != "level") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
+ off >> bs;
+ if (bs != "equalities") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
+ off >> me;
+ Je.resize(me, nJ);
+ ee.resize(me);
+ if (me > 0)
+ for (int i = 0; i < me; ++i) {
+ for (int j = 0; j < nJ; ++j)
+ off >> Je(i, j);
+ off >> ee(i);
+ }
+
+ /* --- Parse inequalities --- */
+ off >> bs;
+ if (bs != "inequalities") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
+ off >> mi;
+ Ji.resize(mi, nJ);
+ eiInf.resize(mi);
+ eiSup.resize(mi);
+ eiBoundSide.resize(mi);
+ if (mi > 0)
+ for (int i = 0; i < mi; ++i) {
+ for (int j = 0; j < nJ; ++j)
+ off >> Ji(i, j);
+ std::string number;
+ eiBoundSide[i] = ConstraintMem::BOUND_VOID;
+ off >> number;
+ if (number != "X") {
+ eiBoundSide[i] = (ConstraintMem::BoundSideType)(
+ eiBoundSide[i] | ConstraintMem::BOUND_INF);
+ eiInf(i) = atof(number.c_str());
+ } else {
+ eiInf(i) = 1e-66;
}
- solver->recordInitialConditions();
- solvers.push_back(solver);
-
+ off >> number;
+ if (number != "X") {
+ eiBoundSide[i] = (ConstraintMem::BoundSideType)(
+ eiBoundSide[i] | ConstraintMem::BOUND_SUP);
+ eiSup(i) = atof(number.c_str());
+ } else {
+ eiSup(i) = 1e-66;
+ }
+ }
+
+ struct timeval t0, t1;
+ double dtsolver;
+ sotDEBUG(15)
+ << "/* ----------------------------------------------------- */"
+ << std::endl;
+ sotDEBUG(15)
+ << "/* ----------------------------------------------------- */"
+ << std::endl;
+ sotDEBUG(15)
+ << "/* ----------------------------------------------------- */"
+ << std::endl;
+ sotDEBUG(5) << "ee" << level << " = " << (MATLAB)ee << endl;
+ sotDEBUG(5) << "eiInf" << level << " = " << (MATLAB)eiInf << endl;
+ sotDEBUG(5) << "eiSup" << level << " = " << (MATLAB)eiSup << endl;
+ sotDEBUG(5) << "Je" << level << " = " << (MATLAB)Je << endl;
+ sotDEBUG(5) << "Ji" << level << " = " << (MATLAB)Ji << endl;
+ gettimeofday(&t0, NULL);
+
+ Jes.push_back(Je);
+ Jis.push_back(Ji);
+ ees.push_back(ee);
+ eiInfs.push_back(eiInf);
+ eiSups.push_back(eiSup);
+ bounds.push_back(eiBoundSide);
+
+ sotDEBUG(1) << "--- Level " << level << std::endl;
+ SolverHierarchicalInequalities *solver =
+ new SolverHierarchicalInequalities(nJ, Qh, Rh, constraintH);
+ solver->initConstraintSize(Je.size1() + Ji.size1());
+ if (solvers.size() == 0)
+ solver->setInitialConditionVoid();
+ else {
+ SolverHierarchicalInequalities *solverPrec = solvers.back();
+ solver->setInitialCondition(solverPrec->u0, solverPrec->rankh);
+ }
+ solver->recordInitialConditions();
+ solvers.push_back(solver);
- solver->solve(Je,ee,Ji,eiInf,eiSup,eiBoundSide);
+ solver->solve(Je, ee, Ji, eiInf, eiSup, eiBoundSide);
- /*!*/gettimeofday(&t1,NULL);
- /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
- /*!*/sotDEBUG(1) << "u" << level << " = " << (MATLAB)solver->u0 << endl;
- /*!*/cout << "dtSOLV_" << level << " = " << dtsolver << "%ms" << endl;
+ /*!*/ gettimeofday(&t1, NULL);
+ /*!*/ dtsolver = (t1.tv_sec - t0.tv_sec) * 1000. +
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.;
+ /*!*/ sotDEBUG(1) << "u" << level << " = " << (MATLAB)solver->u0 << endl;
+ /*!*/ cout << "dtSOLV_" << level << " = " << dtsolver << "%ms" << endl;
- solver->computeDifferentialCondition();
+ solver->computeDifferentialCondition();
#ifdef VP_DEBUG
- solver->printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
+ solver->printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
#endif
+ }
+
+ for (unsigned int repet = 0; repet < 1; ++repet) {
+ cout << "Repet = " << repet << std::endl;
+ struct timeval t0, t1;
+ double dtsolver;
+ constraintH.resize(0);
+ Rh *= 0;
+ Qh.clear();
+ for (unsigned int level = 0; level < solvers.size(); ++level) {
+ gettimeofday(&t0, NULL);
+ sotDEBUG(1) << "--- Level " << level << std::endl;
+ SolverHierarchicalInequalities *solver = solvers[level];
+ if (level == 0)
+ solver->setInitialConditionVoid();
+ else {
+ SolverHierarchicalInequalities *solverPrec = solvers[level - 1];
+ solver->setInitialCondition(solverPrec->u0, solverPrec->rankh);
+ }
- }
-
- for( unsigned int repet=0;repet<1;++repet )
- {
- cout << "Repet = " << repet << std::endl;
- struct timeval t0,t1; double dtsolver;
- constraintH.resize(0);
- Rh*=0;
- Qh.clear();
- for( unsigned int level=0;level<solvers.size();++level )
- {
- gettimeofday(&t0,NULL);
- sotDEBUG(1) << "--- Level " << level << std::endl;
- SolverHierarchicalInequalities * solver = solvers[level];
- if( level==0 ) solver->setInitialConditionVoid();
- else
- {
- SolverHierarchicalInequalities * solverPrec = solvers[level-1];
- solver->setInitialCondition( solverPrec->u0,solverPrec->rankh );
- }
-
- solver->recordInitialConditions();
+ solver->recordInitialConditions();
#ifdef WITH_WARM_START
- solver->warmStart();
+ solver->warmStart();
#endif
- /*!*/gettimeofday(&t1,NULL);
- /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
- /*!*/sotDEBUG(1) << "dtWS_" << level << " = " << dtsolver << "%ms" << endl;
- /*!*/cout << " " << dtsolver ;
- /*!*/gettimeofday(&t0,NULL);
-
- solver->solve( Jes[level],ees[level],Jis[level],
- eiInfs[level],eiSups[level],bounds[level],
- solver->getSlackActiveSet() );
- sotDEBUG(1) << "u" << level << " = " << (MATLAB)solver->u0 << endl;
- /*!*/gettimeofday(&t1,NULL);
- /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
- /*!*/cout << " " << dtsolver ;
- /*!*/sotDEBUG(1) << "dtSOLV_" << level << " = " << dtsolver << "%ms" << endl;
- /*!*/gettimeofday(&t0,NULL);
-
- solver->computeDifferentialCondition();
+ /*!*/ gettimeofday(&t1, NULL);
+ /*!*/ dtsolver = (t1.tv_sec - t0.tv_sec) * 1000. +
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.;
+ /*!*/ sotDEBUG(1) << "dtWS_" << level << " = " << dtsolver << "%ms"
+ << endl;
+ /*!*/ cout << " " << dtsolver;
+ /*!*/ gettimeofday(&t0, NULL);
+
+ solver->solve(Jes[level], ees[level], Jis[level], eiInfs[level],
+ eiSups[level], bounds[level], solver->getSlackActiveSet());
+ sotDEBUG(1) << "u" << level << " = " << (MATLAB)solver->u0 << endl;
+ /*!*/ gettimeofday(&t1, NULL);
+ /*!*/ dtsolver = (t1.tv_sec - t0.tv_sec) * 1000. +
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.;
+ /*!*/ cout << " " << dtsolver;
+ /*!*/ sotDEBUG(1) << "dtSOLV_" << level << " = " << dtsolver << "%ms"
+ << endl;
+ /*!*/ gettimeofday(&t0, NULL);
+
+ solver->computeDifferentialCondition();
#ifdef VP_DEBUG
- solver->printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
+ solver->printDifferentialCondition(sotDEBUGFLOW.outputbuffer);
#endif
- /*!*/gettimeofday(&t1,NULL);
- /*!*/dtsolver = (t1.tv_sec-t0.tv_sec) * 1000. + (t1.tv_usec-t0.tv_usec+0.)/1000. ;
- /*!*/cout << " " << dtsolver ;
- /*!*/sotDEBUG(1) << "dtREC_" << level << " = " << dtsolver << "%ms" << endl;
- }
- std::cout << std::endl;
+ /*!*/ gettimeofday(&t1, NULL);
+ /*!*/ dtsolver = (t1.tv_sec - t0.tv_sec) * 1000. +
+ (t1.tv_usec - t0.tv_usec + 0.) / 1000.;
+ /*!*/ cout << " " << dtsolver;
+ /*!*/ sotDEBUG(1) << "dtREC_" << level << " = " << dtsolver << "%ms"
+ << endl;
}
-
-
- for( unsigned int level=0;level<solvers.size();++level )
- { delete solvers[level]; solvers[level]=NULL; }
- solvers.clear();
+ std::cout << std::endl;
+ }
+
+ for (unsigned int level = 0; level < solvers.size(); ++level) {
+ delete solvers[level];
+ solvers[level] = NULL;
+ }
+ solvers.clear();
}
/* ---------------------------------------------------------- */
-void deparse( std::vector<bubMatrix> Jes,std::vector<bubVector> ees,
- std::vector<bubMatrix> Jis,std::vector<bubVector> eiInfs,
- std::vector<bubVector> eiSups,std::vector<ConstraintMem::BoundSideVector> bounds )
-{
+void deparse(std::vector<bubMatrix> Jes, std::vector<bubVector> ees,
+ std::vector<bubMatrix> Jis, std::vector<bubVector> eiInfs,
+ std::vector<bubVector> eiSups,
+ std::vector<ConstraintMem::BoundSideVector> bounds) {
using namespace std;
cout << "variable size " << Jes[0].size2() << endl;
- for( unsigned int i=0;i<Jes.size();++i )
- {
- bubMatrix & Je = Jes[i];
- bubMatrix & Ji = Jis[i];
- bubVector & ee = ees[i];
- bubVector & eiInf = eiInfs[i];
- bubVector & eiSup = eiSups[i];
- ConstraintMem::BoundSideVector & boundSide = bounds[i];
-
- cout << endl << endl << "level" << endl << endl << "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) << " ";
- 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++;
- }
-
- cout << endl << "inequalities " << nbIneq << endl;
- if(eiInf.size()>0)
- for( unsigned int i=0;i<eiInf.size();++i )
- {
- if( boundSide[i]&ConstraintMem::BOUND_INF )
- {
- for( unsigned int j=0;j<Ji.size2();++j )
- cout << -Ji(i,j) << " ";
- cout << "\t" << " X " << -eiInf(i) << endl;
- }
- if( boundSide[i]&ConstraintMem::BOUND_SUP )
- {
- for( unsigned int j=0;j<Ji.size2();++j )
- cout << Ji(i,j) << " ";
- cout << "\t" << " X " << eiSup(i) << endl;
- }
- }
+ for (unsigned int i = 0; i < Jes.size(); ++i) {
+ bubMatrix &Je = Jes[i];
+ bubMatrix &Ji = Jis[i];
+ bubVector &ee = ees[i];
+ bubVector &eiInf = eiInfs[i];
+ bubVector &eiSup = eiSups[i];
+ ConstraintMem::BoundSideVector &boundSide = bounds[i];
+
+ cout << endl
+ << endl
+ << "level" << endl
+ << endl
+ << "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) << " ";
+ 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++;
}
+
+ cout << endl << "inequalities " << nbIneq << endl;
+ if (eiInf.size() > 0)
+ for (unsigned int i = 0; i < eiInf.size(); ++i) {
+ if (boundSide[i] & ConstraintMem::BOUND_INF) {
+ for (unsigned int j = 0; j < Ji.size2(); ++j)
+ cout << -Ji(i, j) << " ";
+ cout << "\t"
+ << " X " << -eiInf(i) << endl;
+ }
+ if (boundSide[i] & ConstraintMem::BOUND_SUP) {
+ for (unsigned int j = 0; j < Ji.size2(); ++j)
+ cout << Ji(i, j) << " ";
+ cout << "\t"
+ << " X " << eiSup(i) << endl;
+ }
+ }
+ }
sotDEBUG(15) << endl << endl << "end" << endl;
}
-
-void convertDoubleToSingle( const std::string filename )
-{
+void convertDoubleToSingle(const std::string filename) {
using namespace std;
- std::ifstream off( filename.c_str() );
+ std::ifstream off(filename.c_str());
std::string bs;
int nJ;
- int me,mi;
- bubMatrix Je,Ji;
- bubVector ee,eiInf,eiSup;
+ int me, mi;
+ bubMatrix Je, Ji;
+ bubVector ee, eiInf, eiSup;
ConstraintMem::BoundSideVector eiBoundSide;
- off >> bs; if(bs!="variable") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> bs; if(bs!="size") { cerr << "!! '" << bs << "'" << endl; return; }
+ off >> bs;
+ if (bs != "variable") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
+ off >> bs;
+ if (bs != "size") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
off >> nJ;
/* --- Set config file --- */
@@ -284,129 +321,161 @@ void convertDoubleToSingle( const std::string filename )
std::vector<bubVector> eiSups;
std::vector<ConstraintMem::BoundSideVector> bounds;
- for( unsigned int level=0;;++level )
- {
- off >> bs;
- if(bs=="end") { break; }
- else if(bs!="level") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> bs; if(bs!="equalities") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> me;
- Je.resize(me,nJ); ee.resize(me);
- if(me>0)
- for( int i=0;i<me;++i )
- {
- for( int j=0;j<nJ;++j )
- off >> Je(i,j);
- off >> ee(i);
- }
-
- off >> bs; if(bs!="inequalities") { cerr << "!! '" << bs << "'" << endl; return; }
- off >> mi;
- Ji.resize(mi,nJ); eiInf.resize(mi); eiSup.resize(mi); eiBoundSide.resize(mi);
- if(mi>0)
- for( int i=0;i<mi;++i )
- {
- 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;
- if( number!="X" )
- {
- eiBoundSide[i] = (ConstraintMem::BoundSideType)(eiBoundSide[i]|ConstraintMem::BOUND_INF);
- eiInf(i) = atof(number.c_str());
- } else { eiInf(i) = 1e-66; }
- off>>number;
- if( number!="X" )
- {
- eiBoundSide[i] = (ConstraintMem::BoundSideType)(eiBoundSide[i]|ConstraintMem::BOUND_SUP);
- eiSup(i) = atof(number.c_str());
- } else { eiSup(i) = 1e-66; }
- }
- //Ji*=-1; eiInf*=-1;
-
- Jes.push_back(Je);
- ees.push_back(ee);
- Jis.push_back(Ji);
- eiInfs.push_back(eiInf);
- eiSups.push_back(eiSup);
- bounds.push_back(eiBoundSide);
+ for (unsigned int level = 0;; ++level) {
+ off >> bs;
+ if (bs == "end") {
+ break;
+ } else if (bs != "level") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
}
-
- deparse(Jes,ees,Jis,eiInfs,eiSups,bounds);
+ off >> bs;
+ if (bs != "equalities") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
+ off >> me;
+ Je.resize(me, nJ);
+ ee.resize(me);
+ if (me > 0)
+ for (int i = 0; i < me; ++i) {
+ for (int j = 0; j < nJ; ++j)
+ off >> Je(i, j);
+ off >> ee(i);
+ }
+
+ off >> bs;
+ if (bs != "inequalities") {
+ cerr << "!! '" << bs << "'" << endl;
+ return;
+ }
+ off >> mi;
+ Ji.resize(mi, nJ);
+ eiInf.resize(mi);
+ eiSup.resize(mi);
+ eiBoundSide.resize(mi);
+ if (mi > 0)
+ for (int i = 0; i < mi; ++i) {
+ 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;
+ if (number != "X") {
+ eiBoundSide[i] = (ConstraintMem::BoundSideType)(
+ eiBoundSide[i] | ConstraintMem::BOUND_INF);
+ eiInf(i) = atof(number.c_str());
+ } else {
+ eiInf(i) = 1e-66;
+ }
+ off >> number;
+ if (number != "X") {
+ eiBoundSide[i] = (ConstraintMem::BoundSideType)(
+ eiBoundSide[i] | ConstraintMem::BOUND_SUP);
+ eiSup(i) = atof(number.c_str());
+ } else {
+ eiSup(i) = 1e-66;
+ }
+ }
+ // Ji*=-1; eiInf*=-1;
+
+ Jes.push_back(Je);
+ ees.push_back(ee);
+ Jis.push_back(Ji);
+ eiInfs.push_back(eiInf);
+ eiSups.push_back(eiSup);
+ bounds.push_back(eiBoundSide);
+ }
+
+ deparse(Jes, ees, Jis, eiInfs, eiSups, bounds);
}
-
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-void randBound( ConstraintMem::BoundSideVector& M,const unsigned int row)
-{
+void randBound(ConstraintMem::BoundSideVector &M, const unsigned int row) {
M.resize(row);
- for( unsigned int i=0;i<row;++i )
- {
- double c = ((rand()+0.0)/RAND_MAX*2)-1.;
- if( c<0 ) M[i] = ConstraintMem::BOUND_INF;
- else M[i] = ConstraintMem::BOUND_SUP;
- }
+ for (unsigned int i = 0; i < row; ++i) {
+ double c = ((rand() + 0.0) / RAND_MAX * 2) - 1.;
+ if (c < 0)
+ M[i] = ConstraintMem::BOUND_INF;
+ else
+ M[i] = ConstraintMem::BOUND_SUP;
+ }
}
-void randTest( const unsigned int nJ,const bool enableSolve[] )
-{
- bubVector eiInf0(nJ),ee0(1),eiSup0(nJ);
- bubMatrix Ji0(nJ,nJ),Je0(1,nJ);
- ConstraintMem::BoundSideVector bound0(nJ,ConstraintMem::BOUND_INF);
- Ji0.assign( bub::identity_matrix<double>(nJ));
- eiInf0.assign( bub::zero_vector<double>(nJ));
- Je0.assign( bub::zero_matrix<double>(1,nJ));
- //std::fill(ee0.data().begin(),ee0.data().end(),1);
- ee0(0)=0; Je0(0,0)=1;
-
- bubVector ee1,eiInf1,eiSup1;
- bubMatrix Je1,Ji1;
+void randTest(const unsigned int nJ, const bool enableSolve[]) {
+ bubVector eiInf0(nJ), ee0(1), eiSup0(nJ);
+ bubMatrix Ji0(nJ, nJ), Je0(1, nJ);
+ ConstraintMem::BoundSideVector bound0(nJ, ConstraintMem::BOUND_INF);
+ Ji0.assign(bub::identity_matrix<double>(nJ));
+ eiInf0.assign(bub::zero_vector<double>(nJ));
+ Je0.assign(bub::zero_matrix<double>(1, nJ));
+ // std::fill(ee0.data().begin(),ee0.data().end(),1);
+ ee0(0) = 0;
+ Je0(0, 0) = 1;
+
+ bubVector ee1, eiInf1, eiSup1;
+ bubMatrix Je1, Ji1;
ConstraintMem::BoundSideVector bound1;
- randVector(ee1,3); sotDEBUG(15) << "ee1 = " << (MATLAB)ee1 << std::endl;
- randVector(eiInf1,3); sotDEBUG(15) << "eiInf1 = " << (MATLAB)eiInf1 << std::endl;
- randVector(eiSup1,3); sotDEBUG(15) << "eiSup1 = " << (MATLAB)eiSup1 << std::endl;
- randBound(bound1,3);
- randMatrix(Je1,3,nJ); //sotDEBUG(15) << "Je1 = " << (MATLAB)Je1 << std::endl;
- randMatrix(Ji1,3,nJ); sotDEBUG(15) << "Ji1 = " << (MATLAB)Ji1 << std::endl;
- bubMatrix xhi; randMatrix(xhi,1,3); xhi(0,2)=0;
- bub::project(Je1,bub::range(2,3),bub::range(0,nJ)) = bub::prod(xhi,Je1);
+ randVector(ee1, 3);
+ sotDEBUG(15) << "ee1 = " << (MATLAB)ee1 << std::endl;
+ randVector(eiInf1, 3);
+ sotDEBUG(15) << "eiInf1 = " << (MATLAB)eiInf1 << std::endl;
+ randVector(eiSup1, 3);
+ sotDEBUG(15) << "eiSup1 = " << (MATLAB)eiSup1 << std::endl;
+ randBound(bound1, 3);
+ randMatrix(Je1, 3,
+ nJ); // sotDEBUG(15) << "Je1 = " << (MATLAB)Je1 << std::endl;
+ randMatrix(Ji1, 3, nJ);
+ sotDEBUG(15) << "Ji1 = " << (MATLAB)Ji1 << std::endl;
+ bubMatrix xhi;
+ randMatrix(xhi, 1, 3);
+ xhi(0, 2) = 0;
+ bub::project(Je1, bub::range(2, 3), bub::range(0, nJ)) = bub::prod(xhi, Je1);
sotDEBUG(15) << "Je1 = " << (MATLAB)Je1 << std::endl;
- //randMatrix(xhi,3,3);
+ // randMatrix(xhi,3,3);
// bub::noalias(Ji1) = bub::prod(xhi,Je1);
// sotDEBUG(15) << "Ji1 = " << (MATLAB)Ji1 << std::endl;
-
- bubVector ee2,eiInf2,eiSup2;
- bubMatrix Je2,Ji2;
+ bubVector ee2, eiInf2, eiSup2;
+ bubMatrix Je2, Ji2;
ConstraintMem::BoundSideVector bound2;
- randVector(ee2,3); sotDEBUG(15) << "ee2 = " << (MATLAB)ee2 << std::endl;
- randVector(eiInf2,3); sotDEBUG(15) << "eiInf2 = " << (MATLAB)eiInf2 << std::endl;
- randVector(eiSup2,3); sotDEBUG(15) << "eiSup2 = " << (MATLAB)eiSup2 << std::endl;
- randBound(bound1,3);
- randMatrix(Je2,3,nJ); sotDEBUG(15) << "Je2 = " << (MATLAB)Je2 << std::endl;
- randMatrix(Ji2,3,nJ); sotDEBUG(15) << "Ji2 = " << (MATLAB)Ji2 << std::endl;
-
- bubVector ee3,eiInf3,eiSup3;
- bubMatrix Je3,Ji3;
+ randVector(ee2, 3);
+ sotDEBUG(15) << "ee2 = " << (MATLAB)ee2 << std::endl;
+ randVector(eiInf2, 3);
+ sotDEBUG(15) << "eiInf2 = " << (MATLAB)eiInf2 << std::endl;
+ randVector(eiSup2, 3);
+ sotDEBUG(15) << "eiSup2 = " << (MATLAB)eiSup2 << std::endl;
+ randBound(bound1, 3);
+ randMatrix(Je2, 3, nJ);
+ sotDEBUG(15) << "Je2 = " << (MATLAB)Je2 << std::endl;
+ randMatrix(Ji2, 3, nJ);
+ sotDEBUG(15) << "Ji2 = " << (MATLAB)Ji2 << std::endl;
+
+ bubVector ee3, eiInf3, eiSup3;
+ bubMatrix Je3, Ji3;
ConstraintMem::BoundSideVector bound3;
- randVector(ee3,3); sotDEBUG(15) << "ee3 = " << (MATLAB)ee3 << std::endl;
- randVector(eiInf3,3); sotDEBUG(15) << "eiInf3 = " << (MATLAB)eiInf3 << std::endl;
- randVector(eiSup3,3); sotDEBUG(15) << "eiSup3 = " << (MATLAB)eiSup3 << std::endl;
- randBound(bound1,3);
- randMatrix(Je3,3,nJ); sotDEBUG(15) << "Je3 = " << (MATLAB)Je3 << std::endl;
- randMatrix(Ji3,3,nJ); sotDEBUG(15) << "Ji3 = " << (MATLAB)Ji3 << std::endl;
-
- bubVector ee4(nJ),eiInf4(1),eiSup4(1);
- bubMatrix Je4(nJ,nJ),Ji4(1,nJ);
- ConstraintMem::BoundSideVector bound4(1,ConstraintMem::BOUND_INF);
- Je4.assign( bub::identity_matrix<double>(nJ));
- ee4.assign( bub::zero_vector<double>(nJ));
- Ji4.assign( bub::zero_matrix<double>(1,nJ));
- eiInf4.assign( bub::zero_vector<double>(1));
- eiSup4.assign( bub::zero_vector<double>(1));
+ randVector(ee3, 3);
+ sotDEBUG(15) << "ee3 = " << (MATLAB)ee3 << std::endl;
+ randVector(eiInf3, 3);
+ sotDEBUG(15) << "eiInf3 = " << (MATLAB)eiInf3 << std::endl;
+ randVector(eiSup3, 3);
+ sotDEBUG(15) << "eiSup3 = " << (MATLAB)eiSup3 << std::endl;
+ randBound(bound1, 3);
+ randMatrix(Je3, 3, nJ);
+ sotDEBUG(15) << "Je3 = " << (MATLAB)Je3 << std::endl;
+ randMatrix(Ji3, 3, nJ);
+ sotDEBUG(15) << "Ji3 = " << (MATLAB)Ji3 << std::endl;
+
+ bubVector ee4(nJ), eiInf4(1), eiSup4(1);
+ bubMatrix Je4(nJ, nJ), Ji4(1, nJ);
+ ConstraintMem::BoundSideVector bound4(1, ConstraintMem::BOUND_INF);
+ Je4.assign(bub::identity_matrix<double>(nJ));
+ ee4.assign(bub::zero_vector<double>(nJ));
+ Ji4.assign(bub::zero_matrix<double>(1, nJ));
+ eiInf4.assign(bub::zero_vector<double>(1));
+ eiSup4.assign(bub::zero_vector<double>(1));
/* --- Set config file --- */
std::vector<bubMatrix> Jes;
@@ -416,101 +485,111 @@ void randTest( const unsigned int nJ,const bool enableSolve[] )
std::vector<bubVector> eiSups;
std::vector<ConstraintMem::BoundSideVector> bounds;
- if(enableSolve[0])
- {
- Jes.push_back(Je0);
- ees.push_back(ee0);
- Jis.push_back(Ji0);
- eiInfs.push_back(eiInf0);
- eiSups.push_back(eiSup0);
- bounds.push_back(bound0);
- }
-
- if(enableSolve[1])
- {
- Jes.push_back(Je1);
- ees.push_back(ee1);
- Jis.push_back(Ji1);
- eiInfs.push_back(eiInf1);
- eiSups.push_back(eiSup1);
- bounds.push_back(bound1);
- }
- if(enableSolve[2])
- {
- Jes.push_back(Je2);
- ees.push_back(ee2);
- Jis.push_back(Ji2);
- eiInfs.push_back(eiInf2);
- eiSups.push_back(eiSup2);
- bounds.push_back(bound2);
- }
- if(enableSolve[3])
- {
- Jes.push_back(Je3);
- ees.push_back(ee3);
- Jis.push_back(Ji3);
- eiInfs.push_back(eiInf3);
- eiSups.push_back(eiSup3);
- bounds.push_back(bound3);
- }
- if(enableSolve[4])
- {
- Jes.push_back(Je4);
- ees.push_back(ee4);
- Jis.push_back(Ji4);
- eiInfs.push_back(eiInf4);
- eiSups.push_back(eiSup4);
- bounds.push_back(bound4);
- }
- deparse(Jes,ees,Jis,eiInfs,eiSups,bounds);
+ if (enableSolve[0]) {
+ Jes.push_back(Je0);
+ ees.push_back(ee0);
+ Jis.push_back(Ji0);
+ eiInfs.push_back(eiInf0);
+ eiSups.push_back(eiSup0);
+ bounds.push_back(bound0);
+ }
+
+ if (enableSolve[1]) {
+ Jes.push_back(Je1);
+ ees.push_back(ee1);
+ Jis.push_back(Ji1);
+ eiInfs.push_back(eiInf1);
+ eiSups.push_back(eiSup1);
+ bounds.push_back(bound1);
+ }
+ if (enableSolve[2]) {
+ Jes.push_back(Je2);
+ ees.push_back(ee2);
+ Jis.push_back(Ji2);
+ eiInfs.push_back(eiInf2);
+ eiSups.push_back(eiSup2);
+ bounds.push_back(bound2);
+ }
+ if (enableSolve[3]) {
+ Jes.push_back(Je3);
+ ees.push_back(ee3);
+ Jis.push_back(Ji3);
+ eiInfs.push_back(eiInf3);
+ eiSups.push_back(eiSup3);
+ bounds.push_back(bound3);
+ }
+ if (enableSolve[4]) {
+ Jes.push_back(Je4);
+ ees.push_back(ee4);
+ Jis.push_back(Ji4);
+ eiInfs.push_back(eiInf4);
+ eiSups.push_back(eiSup4);
+ bounds.push_back(bound4);
+ }
+ deparse(Jes, ees, Jis, eiInfs, eiSups, bounds);
sotRotationComposedInExtenso Qh(nJ);
bubMatrix Rh;
SolverHierarchicalInequalities::ConstraintList constraintH;
- SolverHierarchicalInequalities solver(nJ,Qh,Rh,constraintH);
- solver.initConstraintSize((40+6+6+6+40)+2);
-
+ SolverHierarchicalInequalities solver(nJ, Qh, Rh, constraintH);
+ solver.initConstraintSize((40 + 6 + 6 + 6 + 40) + 2);
/* ---------------------------------------------------------- */
- 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);
- sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
- sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
- sotDEBUG(15) << "/* ----------------------------------------------------- */" << std::endl;
- 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);
- 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[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);
+ sotDEBUG(15) << "/* ----------------------------------------------------- */"
+ << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */"
+ << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */"
+ << std::endl;
+ 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);
+ sotDEBUG(15) << "/* ----------------------------------------------------- */"
+ << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */"
+ << std::endl;
+ sotDEBUG(15) << "/* ----------------------------------------------------- */"
+ << std::endl;
+ if (enableSolve[4])
+ solver.solve(Je4, ee4, Ji4, eiInf4, eiSup4, bound4);
/* ---------------------------------------------------------- */
-
return;
}
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
/* ---------------------------------------------------------- */
-int main( void )
-{
+int main(void) {
- // convertDoubleToSingle("/home/nmansard/src/StackOfTasks/tests/tools/testFR.txt"); exit(0);
+ // convertDoubleToSingle("/home/nmansard/src/StackOfTasks/tests/tools/testFR.txt");
+ // exit(0);
#ifndef VP_DEBUG
#ifndef WITH_CHRONO
- for(int i=0;i<10;++i)
+ for (int i = 0; i < 10; ++i)
#endif
#endif
- parseTest("/home/nmansard/src/StackOfTasks/tests//t.txt");
-// bool enable [5] ={ 1,1,0,0,0};
-// randTest(9,enable );
+ parseTest("/home/nmansard/src/StackOfTasks/tests//t.txt");
+ // bool enable [5] ={ 1,1,0,0,0};
+ // randTest(9,enable );
}
diff --git a/unitTesting/sot/tsot.cpp b/unitTesting/sot/tsot.cpp
index 5cee32b6..6f035940 100644
--- a/unitTesting/sot/tsot.cpp
+++ b/unitTesting/sot/tsot.cpp
@@ -12,39 +12,38 @@
/* -------------------------------------------------------------------------- */
#include <iostream>
//#include <sot/core/sot-h.hh>
-#include <sot/core/feature-visual-point.hh>
-#include <sot/core/feature-abstract.hh>
+#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>
-#include <sot/core/task.hh>
+#include <sot/core/feature-abstract.hh>
+#include <sot/core/feature-visual-point.hh>
#include <sot/core/gain-adaptive.hh>
-#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/task.hh>
using namespace std;
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();
+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();
return J;
}
-int main( void )
-{
- sotDEBUGF( "# In {" );
+int main(void) {
+ sotDEBUGF("# In {");
srand(12);
- dynamicgraph::Matrix Jq(6,6); Jq.setIdentity();
+ dynamicgraph::Matrix Jq(6, 6);
+ Jq.setIdentity();
- dynamicgraph::Vector p1xy(2); p1xy(0)=1.; p1xy(1)=-2;
+ dynamicgraph::Vector p1xy(2);
+ p1xy(0) = 1.;
+ p1xy(1) = -2;
sotDEBUGF("Create feature");
- FeatureVisualPoint * p1 = new FeatureVisualPoint("p1");
- FeatureVisualPoint * p1des = new FeatureVisualPoint("p1des");
-
+ FeatureVisualPoint *p1 = new FeatureVisualPoint("p1");
+ FeatureVisualPoint *p1des = new FeatureVisualPoint("p1des");
p1->articularJacobianSIN.setReference(&Jq);
p1->selectionSIN = Flags(true);
@@ -56,24 +55,24 @@ int main( void )
sotDEBUGF("Create Task");
// sotDEBUG(0) << dynamicgraph::MATLAB;
- Task * task = new Task("task");
+ Task *task = new Task("task");
task->addFeature(*p1);
task->addFeature(*p1);
- GainAdaptive * lambda = new GainAdaptive("g");
- lambda->errorSIN.plug( &task->errorSOUT );
+ GainAdaptive *lambda = new GainAdaptive("g");
+ lambda->errorSIN.plug(&task->errorSOUT);
- task->controlGainSIN.plug( &lambda->gainSOUT );
+ task->controlGainSIN.plug(&lambda->gainSOUT);
task->dampingGainSINOUT = .1;
task->controlSelectionSIN = Flags(true);
- task->jacobianSOUT.display(cout)<<endl;
- task->jacobianSOUT.displayDependencies(cout)<<endl;
+ task->jacobianSOUT.display(cout) << endl;
+ task->jacobianSOUT.displayDependencies(cout) << endl;
- sotDEBUG(0) << "J"<< task->jacobianSOUT(2);
- sotDEBUG(0) <<"e"<< task->errorSOUT(2) <<endl;
+ sotDEBUG(0) << "J" << task->jacobianSOUT(2);
+ sotDEBUG(0) << "e" << task->errorSOUT(2) << endl;
- sotDEBUGF( "# Out }" );
+ sotDEBUGF("# Out }");
return 0;
}
diff --git a/unitTesting/task/test_flags.cpp b/unitTesting/task/test_flags.cpp
index d66a4fee..1ea183e6 100644
--- a/unitTesting/task/test_flags.cpp
+++ b/unitTesting/task/test_flags.cpp
@@ -10,56 +10,80 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <sot/core/flags.hh>
#include <iostream>
+#include <sot/core/flags.hh>
#include <sstream>
using namespace std;
using namespace dynamicgraph::sot;
-int main( void )
-{
- cout <<"Entering test" <<endl;
- Flags f1(128*112+84);
+int main(void) {
+ cout << "Entering test" << endl;
+ Flags f1(128 * 112 + 84);
Flags f2(198);
- cout<<"f1 "<<"\t"<<f1<<endl;
- cout<<"f2 "<<"\t"<<f2<<endl;
+ cout << "f1 "
+ << "\t" << f1 << endl;
+ cout << "f2 "
+ << "\t" << f2 << endl;
- cout<<endl;
- cout<<"1|2 "<<"\t"<<(f1|f2)<<endl;
- cout<<"1&2 "<<"\t"<<(f1&f2)<<endl;
- cout<<"TRUE "<<"\t"<<(Flags(true))<<endl;
- cout<<"1&TRUE"<<"\t"<<(f1&Flags(true))<<endl;
- cout<<"1&!2 "<<"\t"<<((!f2)&f1)<<endl;
- cout<<"1XOR2 "<<"\t"<<(((!f2)&f1)|((!f1)&f2))<<endl;
+ cout << endl;
+ cout << "1|2 "
+ << "\t" << (f1 | f2) << endl;
+ cout << "1&2 "
+ << "\t" << (f1 & f2) << endl;
+ cout << "TRUE "
+ << "\t" << (Flags(true)) << endl;
+ cout << "1&TRUE"
+ << "\t" << (f1 & Flags(true)) << endl;
+ cout << "1&!2 "
+ << "\t" << ((!f2) & f1) << endl;
+ cout << "1XOR2 "
+ << "\t" << (((!f2) & f1) | ((!f1) & f2)) << endl;
- cout<<endl;
- cout<<"f1 "<<"\t"<<f1<<endl;
- cout<<"!2 "<<"\t"<<!f2<<endl;
- cout<<"1|!2 "<<"\t"<<(f1|(!f2))<<endl;
+ cout << endl;
+ cout << "f1 "
+ << "\t" << f1 << endl;
+ cout << "!2 "
+ << "\t" << !f2 << endl;
+ cout << "1|!2 "
+ << "\t" << (f1 | (!f2)) << endl;
- cout<<endl;
- if( f1&f2 ) cout<<"TRUE"; else cout<<"FALSE"; cout<<endl;
- if( f1&Flags() ) cout<<"TRUE"; else cout<<"FALSE"; cout<<endl;
+ cout << endl;
+ if (f1 & f2)
+ cout << "TRUE";
+ else
+ cout << "FALSE";
+ cout << endl;
+ if (f1 & Flags())
+ cout << "TRUE";
+ else
+ cout << "FALSE";
+ cout << endl;
- cout<<endl;
- cout<<"f1>>3 "<<"\t"<<Flags(f1>>3)<<endl;
- cout<<"f1>>5 "<<"\t"<<Flags(f1>>5)<<endl;
+ cout << endl;
+ cout << "f1>>3 "
+ << "\t" << Flags(f1 >> 3) << endl;
+ cout << "f1>>5 "
+ << "\t" << Flags(f1 >> 5) << endl;
- cout<<"f1 byte per byte:";
- for(int i=0;i<16;++i) {if(! (i%8)) cout<<" "; cout << f1(i); } cout<<endl;
+ cout << "f1 byte per byte:";
+ for (int i = 0; i < 16; ++i) {
+ if (!(i % 8))
+ cout << " ";
+ cout << f1(i);
+ }
+ cout << endl;
- cout<<endl;
- cout<<"L1 \t"<<FLAG_LINE_1<<endl;
- cout<<"L4 \t"<<FLAG_LINE_4<<endl;
- cout<<"L8 \t"<<FLAG_LINE_8<<endl;
- cout<<endl;
+ cout << endl;
+ cout << "L1 \t" << FLAG_LINE_1 << endl;
+ cout << "L4 \t" << FLAG_LINE_4 << endl;
+ cout << "L8 \t" << FLAG_LINE_8 << endl;
+ cout << endl;
istringstream iss("00101");
Flags flread;
iss >> flread;
- cout<<flread<<endl<<endl;
-
+ cout << flread << endl << endl;
return 0;
}
diff --git a/unitTesting/task/test_gain.cpp b/unitTesting/task/test_gain.cpp
index a51d4fc5..da3c7933 100644
--- a/unitTesting/task/test_gain.cpp
+++ b/unitTesting/task/test_gain.cpp
@@ -11,53 +11,46 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <dynamic-graph/signal.h>
#include <dynamic-graph/linear-algebra.h>
-#include <sot/core/gain-adaptive.hh>
+#include <dynamic-graph/signal.h>
#include <iostream>
+#include <sot/core/gain-adaptive.hh>
using namespace std;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
-
-class DummyClass
-{
+class DummyClass {
public:
dynamicgraph::Vector err;
- dynamicgraph::Vector& getError( dynamicgraph::Vector& res,int t )
- {
- cout << "Dummy::getError ["<< t<< "] "<<endl;
- return res=err;
+ dynamicgraph::Vector &getError(dynamicgraph::Vector &res, int t) {
+ cout << "Dummy::getError [" << t << "] " << endl;
+ return res = err;
}
-
};
DummyClass dummy;
-
-
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-int main( void )
-{
+int main(void) {
dummy.err.resize(3);
dummy.err.fill(3);
- GainAdaptive * gain = new GainAdaptive("gain",4,1,5);
+ GainAdaptive *gain = new GainAdaptive("gain", 4, 1, 5);
- Signal<dynamicgraph::Vector,int> errSig("test");
- errSig.setFunction( boost::bind(&DummyClass::getError,dummy,_1,_2) );
+ Signal<dynamicgraph::Vector, int> errSig("test");
+ errSig.setFunction(boost::bind(&DummyClass::getError, dummy, _1, _2));
- gain->errorSIN.plug( &errSig );
- cout <<"Appel of errSig and display of the result." << endl;
- cout << errSig(0) <<endl;
+ gain->errorSIN.plug(&errSig);
+ cout << "Appel of errSig and display of the result." << endl;
+ cout << errSig(0) << endl;
- Signal<double,int> &gainSig = gain->gainSOUT;
+ Signal<double, int> &gainSig = gain->gainSOUT;
- cout <<"Compute gain from Gain Signal and display."<< endl;
+ cout << "Compute gain from Gain Signal and display." << endl;
cout << gainSig(0) << endl;
return 0;
diff --git a/unitTesting/task/test_multi_bound.cpp b/unitTesting/task/test_multi_bound.cpp
index b4416bea..e1937e4a 100644
--- a/unitTesting/task/test_multi_bound.cpp
+++ b/unitTesting/task/test_multi_bound.cpp
@@ -10,56 +10,75 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <sot/core/multi-bound.hh>
#include <iostream>
-#include <sstream>
#include <sot/core/debug.hh>
+#include <sot/core/multi-bound.hh>
+#include <sstream>
using namespace std;
using namespace dynamicgraph::sot;
-int main( void )
-{
- DebugTrace::openFile();
+int main(void) {
+ DebugTrace::openFile();
- MultiBound mbs(1.2), mbdi(3.4,MultiBound::BOUND_INF)
- ,mbds(5.6, MultiBound::BOUND_SUP),mbdb(-7.8,9.10);
+ MultiBound mbs(1.2), mbdi(3.4, MultiBound::BOUND_INF),
+ mbds(5.6, MultiBound::BOUND_SUP), mbdb(-7.8, 9.10);
cout << "mbs =" << mbs << std::endl;
cout << "mbdi=" << mbdi << std::endl;
cout << "mbds=" << mbds << std::endl;
cout << "mbdb=" << mbdb << std::endl;
{
- ostringstream oss; istringstream iss;
- oss.str(""); oss << mbs; iss.str(oss.str()); iss.seekg(0);
- //{char strbuf[256]; iss.getline(strbuf,256); cout << "#"<<strbuf<<"#"<<std::endl;}
+ ostringstream oss;
+ istringstream iss;
+ oss.str("");
+ oss << mbs;
+ iss.str(oss.str());
+ iss.seekg(0);
+ //{char strbuf[256]; iss.getline(strbuf,256); cout <<
+ //"#"<<strbuf<<"#"<<std::endl;}
iss >> mbs;
cout << oss.str() << "=> mbs =" << mbs << std::endl;
}
{
- ostringstream oss; istringstream iss;
- oss.str(""); oss << mbdi; iss.str(oss.str()); iss.seekg(0);
+ ostringstream oss;
+ istringstream iss;
+ oss.str("");
+ oss << mbdi;
+ iss.str(oss.str());
+ iss.seekg(0);
iss >> mbdi;
cout << oss.str() << "=> mbdi =" << mbdi << std::endl;
}
{
- ostringstream oss; istringstream iss;
- oss.str(""); oss << mbds; iss.str(oss.str()); iss.seekg(0);
+ ostringstream oss;
+ istringstream iss;
+ oss.str("");
+ oss << mbds;
+ iss.str(oss.str());
+ iss.seekg(0);
iss >> mbds;
cout << oss.str() << "=> mbds =" << mbds << std::endl;
}
{
- ostringstream oss; istringstream iss;
- oss.str(""); oss << mbdb; iss.seekg(0); iss.str(oss.str());
+ ostringstream oss;
+ istringstream iss;
+ oss.str("");
+ oss << mbdb;
+ iss.seekg(0);
+ iss.str(oss.str());
iss >> mbdb;
cout << oss.str() << "=> mbdb =" << mbdb << std::endl;
}
- ostringstream oss; istringstream iss;
- oss << "[4](" << mbs << "," << mbdi << "," << mbds << "," << mbdb << ")" << std::endl;
+ ostringstream oss;
+ istringstream iss;
+ oss << "[4](" << mbs << "," << mbdi << "," << mbds << "," << mbdb << ")"
+ << std::endl;
iss.str(oss.str());
- VectorMultiBound vmb; iss >> vmb;
+ VectorMultiBound vmb;
+ iss >> vmb;
cout << "vmb4 = " << vmb << std::endl;
return 0;
diff --git a/unitTesting/task/test_task.cpp b/unitTesting/task/test_task.cpp
index 75310344..b356db5f 100644
--- a/unitTesting/task/test_task.cpp
+++ b/unitTesting/task/test_task.cpp
@@ -12,38 +12,40 @@
/* -------------------------------------------------------------------------- */
#include <iostream>
-#include <sot/core/sot.hh>
-#include <sot/core/feature-visual-point.hh>
-#include <sot/core/feature-abstract.hh>
+#include <dynamic-graph/linear-algebra.h>
#include <sot/core/debug.hh>
-#include <sot/core/task.hh>
+#include <sot/core/feature-abstract.hh>
+#include <sot/core/feature-visual-point.hh>
#include <sot/core/gain-adaptive.hh>
-#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/sot.hh>
+#include <sot/core/task.hh>
using namespace std;
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();
+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();
return J;
}
-int main( void )
-{
+int main(void) {
srand(12);
- dynamicgraph::Matrix Jq(6,6); Jq.setIdentity();
+ dynamicgraph::Matrix Jq(6, 6);
+ Jq.setIdentity();
dynamicgraph::Vector p1xy(6);
- p1xy(0)=1.; p1xy(1)=-2; p1xy(2)=1.;
- p1xy(3)=1.; p1xy(4)=-2; p1xy(5)=1.;
-
+ p1xy(0) = 1.;
+ p1xy(1) = -2;
+ p1xy(2) = 1.;
+ p1xy(3) = 1.;
+ p1xy(4) = -2;
+ p1xy(5) = 1.;
+
sotDEBUGF("Create feature");
- FeatureVisualPoint * p1 = new FeatureVisualPoint("p1");
- FeatureVisualPoint * p1des = new FeatureVisualPoint("p1d");
+ FeatureVisualPoint *p1 = new FeatureVisualPoint("p1");
+ FeatureVisualPoint *p1des = new FeatureVisualPoint("p1d");
p1->articularJacobianSIN.setReference(&Jq);
p1->selectionSIN = Flags(true);
@@ -55,21 +57,21 @@ int main( void )
sotDEBUGF("Create Task");
// sotDEBUG(0) << dynamicgraph::MATLAB;
- Task * task = new Task("t");
+ Task *task = new Task("t");
task->addFeature(*p1);
- GainAdaptive * lambda = new GainAdaptive("g");
- lambda->errorSIN.plug( &task->errorSOUT );
+ GainAdaptive *lambda = new GainAdaptive("g");
+ lambda->errorSIN.plug(&task->errorSOUT);
- task->controlGainSIN.plug( &lambda->gainSOUT );
+ task->controlGainSIN.plug(&lambda->gainSOUT);
task->dampingGainSINOUT = .1;
task->controlSelectionSIN = Flags(true);
- task->jacobianSOUT.display(cout)<<endl;
- task->jacobianSOUT.displayDependencies(cout)<<endl;
+ task->jacobianSOUT.display(cout) << endl;
+ task->jacobianSOUT.displayDependencies(cout) << endl;
// sotDEBUG(0) << dynamicgraph::MATLAB << "J"<< task->jacobianSOUT(2);
- sotDEBUG(0) <<"e"<< task->errorSOUT(2) <<endl;
+ sotDEBUG(0) << "e" << task->errorSOUT(2) << endl;
return 0;
}
diff --git a/unitTesting/tools/plugin.cc b/unitTesting/tools/plugin.cc
index e43006e0..18db7fc5 100644
--- a/unitTesting/tools/plugin.cc
+++ b/unitTesting/tools/plugin.cc
@@ -14,67 +14,61 @@
// 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
-{
+class Plugin : public PluginAbstract {
protected:
- AbstractSotExternalInterface * sotController_;
+ AbstractSotExternalInterface *sotController_;
public:
- Plugin() {};
- ~Plugin() {};
+ Plugin(){};
+ ~Plugin(){};
- void Initialization(std::string &dynamicLibraryName)
- {
+ void Initialization(std::string &dynamicLibraryName) {
// Load the SotRobotBipedController library.
- void * SotRobotControllerLibrary = dlopen(dynamicLibraryName.c_str(),
- RTLD_GLOBAL | RTLD_NOW);
+ void *SotRobotControllerLibrary =
+ dlopen(dynamicLibraryName.c_str(), RTLD_GLOBAL | RTLD_NOW);
if (!SotRobotControllerLibrary) {
std::cerr << "Cannot load library: " << dlerror() << '\n';
- return ;
+ return;
}
// reset errors
dlerror();
// Load the symbols.
- createSotExternalInterface_t * createRobotController =
- (createSotExternalInterface_t *) dlsym(SotRobotControllerLibrary,
- "createSotExternalInterface");
- const char* dlsym_error = dlerror();
+ createSotExternalInterface_t *createRobotController =
+ (createSotExternalInterface_t *)dlsym(SotRobotControllerLibrary,
+ "createSotExternalInterface");
+ const char *dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
- return ;
+ return;
}
/*
destroySotExternalInterface_t * destroyRobotController =
(destroySotExternalInterface_t *) dlsym(SotRobotControllerLibrary,
- "destroySotExternalInterface");
+ "destroySotExternalInterface");
*/
dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
- return ;
+ return;
}
// Create robot-controller
sotController_ = createRobotController();
- cout <<"Went out from Initialization." << endl;
+ cout << "Went out from Initialization." << endl;
}
};
-extern "C"
-{
- PluginAbstract * createPlugin()
- {
- return new Plugin;
- }
+extern "C" {
+PluginAbstract *createPlugin() { return new Plugin; }
}
diff --git a/unitTesting/tools/plugin.hh b/unitTesting/tools/plugin.hh
index e8be8528..8c6b819c 100644
--- a/unitTesting/tools/plugin.hh
+++ b/unitTesting/tools/plugin.hh
@@ -1,13 +1,12 @@
#ifndef _PLUGIN_HH_
#define _PLUGIN_HH_
-class PluginAbstract
-{
+class PluginAbstract {
public:
- PluginAbstract() {};
- virtual ~PluginAbstract() {};
- virtual void Initialization(std::string &astr)=0;
+ PluginAbstract(){};
+ virtual ~PluginAbstract(){};
+ virtual void Initialization(std::string &astr) = 0;
};
-typedef PluginAbstract* createPlugin_t();
+typedef PluginAbstract *createPlugin_t();
#endif /* _PLUGIN_HH_ */
diff --git a/unitTesting/tools/test_abstract_interface.cpp b/unitTesting/tools/test_abstract_interface.cpp
index f6b01138..7b85553e 100644
--- a/unitTesting/tools/test_abstract_interface.cpp
+++ b/unitTesting/tools/test_abstract_interface.cpp
@@ -26,21 +26,18 @@ namespace po = boost::program_options;
class PluginLoader {
protected:
- PluginAbstract * sotController_;
+ PluginAbstract *sotController_;
po::variables_map vm_;
std::string dynamicLibraryName_;
public:
- PluginLoader() {};
- ~PluginLoader() {};
-
- int parseOptions(int argc, char *argv[])
- { po::options_description desc("Allowed options");
- desc.add_options()
- ("help", "produce help message")
- ("input-file", po::value<string>(), "library to load")
- ;
+ PluginLoader(){};
+ ~PluginLoader(){};
+ int parseOptions(int argc, char *argv[]) {
+ po::options_description desc("Allowed options");
+ desc.add_options()("help", "produce help message")(
+ "input-file", po::value<string>(), "library to load");
po::store(po::parse_command_line(argc, argv, desc), vm_);
po::notify(vm_);
@@ -52,48 +49,44 @@ public:
if (!vm_.count("input-file")) {
cout << "No filename specified\n";
return -1;
- }
- else
- dynamicLibraryName_= vm_["input-file"].as< string >();
+ } else
+ dynamicLibraryName_ = vm_["input-file"].as<string>();
Initialization();
return 0;
}
- void Initialization()
- {
+ void Initialization() {
// Load the SotRobotBipedController library.
- void * SotRobotControllerLibrary = dlopen( "libpluginabstract.so",
- RTLD_LAZY | RTLD_LOCAL );
+ void *SotRobotControllerLibrary =
+ dlopen("libpluginabstract.so", RTLD_LAZY | RTLD_LOCAL);
if (!SotRobotControllerLibrary) {
std::cerr << "Cannot load library: " << dlerror() << '\n';
- return ;
+ return;
}
// reset errors
dlerror();
// Load the symbols.
- createPlugin_t * createPlugin =
- (createPlugin_t *) dlsym(SotRobotControllerLibrary,
- "createPlugin");
- const char* dlsym_error = dlerror();
+ createPlugin_t *createPlugin =
+ (createPlugin_t *)dlsym(SotRobotControllerLibrary, "createPlugin");
+ const char *dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
- return ;
+ return;
}
// Create robot-controller
sotController_ = createPlugin();
// std::string s="libsot-hrp2-14-controller.so";
sotController_->Initialization(dynamicLibraryName_);
- cout <<"Went out from Initialization." << endl;
+ cout << "Went out from Initialization." << endl;
}
};
-int main(int argc, char *argv[])
-{
+int main(int argc, char *argv[]) {
PluginLoader aPluginLoader;
- aPluginLoader.parseOptions(argc,argv);
+ aPluginLoader.parseOptions(argc, argv);
}
diff --git a/unitTesting/tools/test_boost.cpp b/unitTesting/tools/test_boost.cpp
index 44156715..f42f5863 100644
--- a/unitTesting/tools/test_boost.cpp
+++ b/unitTesting/tools/test_boost.cpp
@@ -10,12 +10,12 @@
#ifndef WIN32
#include <sys/time.h>
#endif
+#include <dynamic-graph/linear-algebra.h>
#include <iostream>
#include <sot/core/debug.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/matrix-svd.hh>
#include <sot/core/memory-task-sot.hh>
-#include <dynamic-graph/linear-algebra.h>
#ifndef WIN32
#include <unistd.h>
@@ -27,17 +27,21 @@
using namespace dynamicgraph::sot;
using namespace std;
-#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 << " - " << t0##_##name.tv_usec << std::endl
-#define STOP_CHRONO(name,commentaire) \
- gettimeofday(&t1##_##name,NULL); \
- dt##_##name = ( (double)(t1##_##name.tv_sec-t0##_##name.tv_sec) * 1000. \
- + (double)(t1##_##name.tv_usec-t0##_##name.tv_usec) / 1000. );\
- sotDEBUG(25) << "t1 "<<#name<<": "<< t1##_##name.tv_sec << " - " << t1##_##name.tv_usec << std::endl; \
- sotDEBUG(1) << "Time spent "<<#name " " commentaire<<" = " << dt##_##name << std::endl
+#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 << " - " \
+ << t0##_##name.tv_usec << std::endl
+#define STOP_CHRONO(name, commentaire) \
+ gettimeofday(&t1##_##name, NULL); \
+ dt##_##name = ((double)(t1##_##name.tv_sec - t0##_##name.tv_sec) * 1000. + \
+ (double)(t1##_##name.tv_usec - t0##_##name.tv_usec) / 1000.); \
+ sotDEBUG(25) << "t1 " << #name << ": " << t1##_##name.tv_sec << " - " \
+ << t1##_##name.tv_usec << std::endl; \
+ sotDEBUG(1) << "Time spent " << #name " " commentaire << " = " \
+ << dt##_##name << std::endl
/* ----------------------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
@@ -46,17 +50,18 @@ using namespace std;
double timerCounter;
-// static void inverseCounter( ublas::matrix<double>& matrix, dynamicgraph::Matrix& invMatrix )
+// static void inverseCounter( ublas::matrix<double>& matrix,
+// dynamicgraph::Matrix& invMatrix )
// {
// INIT_CHRONO(inv);
-
// ublas::matrix<double,ublas::column_major> I = matrix;
-// ublas::matrix<double,ublas::column_major> U(matrix.size1(),matrix.size1());
-// ublas::matrix<double,ublas::column_major> VT(matrix.size2(),matrix.size2());
-// ublas::vector<double> s(std::min(matrix.size1(),matrix.size2()));
-// char Jobu='A'; /* Compute complete U Matrix */
-// char Jobvt='A'; /* Compute complete VT Matrix */
+// ublas::matrix<double,ublas::column_major>
+// U(matrix.size1(),matrix.size1());
+// ublas::matrix<double,ublas::column_major>
+// VT(matrix.size2(),matrix.size2()); ublas::vector<double>
+// s(std::min(matrix.size1(),matrix.size2())); char Jobu='A'; /* Compute
+// complete U Matrix */ char Jobvt='A'; /* Compute complete VT Matrix */
// char Lw; Lw='O'; /* Compute the optimal size for the working vector */
// #ifdef WITH_OPENHRP
@@ -118,177 +123,183 @@ double timerCounter;
/* ----------------------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
-int main( int argc,char** argv )
-{
- 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]);
- unsigned int c=30;if( argc>2 ) c=atoi(argv[2]);
- static const int BENCH = 100;
-
- dynamicgraph::Matrix M(r,c);
- dynamicgraph::Matrix M1(r,c);
- dynamicgraph::Matrix Minv(c,r);
-
- dynamicgraph::Matrix U,V,S;
-
- 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.;
- nbzeros++ ;}
- else
- for( unsigned int i=0;i<r;++i )
- M(i,j) = (rand()+1.) / RAND_MAX*2-1;
- }
- 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 ;
-
- //sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
- sotDEBUG(15) <<"M1 = " << M1<<endl;
- sotDEBUG(5) <<"Nb zeros = " << nbzeros<<endl;
+int main(int argc, char **argv) {
+ 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]);
+ unsigned int c = 30;
+ if (argc > 2)
+ c = atoi(argv[2]);
+ static const int BENCH = 100;
+
+ dynamicgraph::Matrix M(r, c);
+ dynamicgraph::Matrix M1(r, c);
+ dynamicgraph::Matrix Minv(c, r);
+
+ dynamicgraph::Matrix U, V, S;
+
+ 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.;
+ nbzeros++;
+ } else
+ for (unsigned int i = 0; i < r; ++i)
+ M(i, j) = (rand() + 1.) / RAND_MAX * 2 - 1;
+ }
+ 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 ;
+
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
+ sotDEBUG(15) << "M1 = " << M1 << endl;
+ sotDEBUG(5) << "Nb zeros = " << nbzeros << endl;
INIT_CHRONO(inv);
START_CHRONO(inv);
- for( int i=0;i<BENCH;++i ) Eigen::pseudoInverse(M, Minv);
- STOP_CHRONO(inv,"init");
- sotDEBUG(15) <<"Minv = " << Minv <<endl;
+ for (int i = 0; i < BENCH; ++i)
+ Eigen::pseudoInverse(M, Minv);
+ STOP_CHRONO(inv, "init");
+ sotDEBUG(15) << "Minv = " << Minv << endl;
START_CHRONO(inv);
- for( int i=0;i<BENCH;++i ) Eigen::pseudoInverse(M, Minv );
- STOP_CHRONO(inv,"M+standard");
+ for (int i = 0; i < BENCH; ++i)
+ Eigen::pseudoInverse(M, Minv);
+ STOP_CHRONO(inv, "M+standard");
cout << dt_inv << endl;
-// START_CHRONO(inv);
-// for( int i=0;i<BENCH;++i ) M.pseudoInverse( Minv,1e-6,&U,&S,&V );
-// STOP_CHRONO(inv,"M+");
-// sotDEBUG(15) << dynamicgraph::MATLAB <<"Minv = "<< Minv <<endl;
-
-// timerCounter=0;
-// START_CHRONO(inv);
-// for( int i=0;i<BENCH;++i ) inverseCounter( M1.matrix,Minv );
-// STOP_CHRONO(inv,"M1+");
-// sotDEBUG(5) << "Counter: " << timerCounter << endl;
-// sotDEBUG(15) << dynamicgraph::MATLAB <<"M1inv = "<< Minv <<endl;
-
-// dynamicgraph::Matrix M1diag = U.transpose()*M1*V;
-// timerCounter=0;
-// START_CHRONO(inv);
-// for( int i=0;i<BENCH;++i ) inverseCounter( M1diag.matrix,Minv );
-// STOP_CHRONO(inv,"M1diag+");
-// sotDEBUG(5) << "Counter: " << timerCounter << endl;
-// sotDEBUG(8) << dynamicgraph::MATLAB <<"M1diag = "<< M1diag <<endl;
-// sotDEBUG(15) << dynamicgraph::MATLAB <<"M1diaginv = "<< Minv <<endl;
+ // START_CHRONO(inv);
+ // for( int i=0;i<BENCH;++i ) M.pseudoInverse( Minv,1e-6,&U,&S,&V );
+ // STOP_CHRONO(inv,"M+");
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"Minv = "<< Minv <<endl;
+
+ // timerCounter=0;
+ // START_CHRONO(inv);
+ // for( int i=0;i<BENCH;++i ) inverseCounter( M1.matrix,Minv );
+ // STOP_CHRONO(inv,"M1+");
+ // sotDEBUG(5) << "Counter: " << timerCounter << endl;
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"M1inv = "<< Minv <<endl;
+
+ // dynamicgraph::Matrix M1diag = U.transpose()*M1*V;
+ // timerCounter=0;
+ // START_CHRONO(inv);
+ // for( int i=0;i<BENCH;++i ) inverseCounter( M1diag.matrix,Minv );
+ // STOP_CHRONO(inv,"M1diag+");
+ // sotDEBUG(5) << "Counter: " << timerCounter << endl;
+ // sotDEBUG(8) << dynamicgraph::MATLAB <<"M1diag = "<< M1diag <<endl;
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"M1diaginv = "<< Minv <<endl;
START_CHRONO(inv);
- std::list< unsigned int > nonzeros;
+ std::list<unsigned int> nonzeros;
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);
- if( sumsq > 1e-6 ) { nonzeros.push_back(j); parc++; }
- }
-
- Mcreuse.resize( r,parc );
- }
-
-
- //dynamicgraph::Matrix Mcreuse( r,parc );
-
- parc=0;
- for( std::list< unsigned int >::iterator iter=nonzeros.begin();
- iter!=nonzeros.end();++iter )
- {
- for( unsigned int i=0;i<r;++i ){ Mcreuse(i,parc) = M(i,*iter);}
- parc++;
- }
-
- //dynamicgraph::Matrix Mcreuseinv( Mcreuse.nbCols(),r );
- Mcreuseinv.resize( Mcreuse.cols(),r );
- Eigen::pseudoInverse(Mcreuse, Mcreuseinv );
- parc=0;
- 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);
- parc++;
- }
-
- if(!ib)
- {
- // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
- // sotDEBUG(15) << dynamicgraph::MATLAB <<"Mcreuse = "<< Mcreuse <<endl;
- // sotDEBUG(15) << dynamicgraph::MATLAB <<"Minvnc = "<< Minv <<endl;
- }
+ 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);
+ if (sumsq > 1e-6) {
+ nonzeros.push_back(j);
+ parc++;
+ }
+ }
+
+ Mcreuse.resize(r, parc);
+ }
+ // dynamicgraph::Matrix Mcreuse( r,parc );
+ parc = 0;
+ for (std::list<unsigned int>::iterator iter = nonzeros.begin();
+ iter != nonzeros.end(); ++iter) {
+ for (unsigned int i = 0; i < r; ++i) {
+ Mcreuse(i, parc) = M(i, *iter);
+ }
+ parc++;
}
- STOP_CHRONO(inv,"M+creuse");
- //sotDEBUG(15) << dynamicgraph::MATLAB <<"Minv = "<< Minv <<endl;
- {
+ // dynamicgraph::Matrix Mcreuseinv( Mcreuse.nbCols(),r );
+ Mcreuseinv.resize(Mcreuse.cols(), r);
+ Eigen::pseudoInverse(Mcreuse, Mcreuseinv);
+ parc = 0;
+ 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);
+ parc++;
+ }
- 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);
- if( sumsq > 1e-6 ) { nonzeros.push_back(j); parc++; }
- }
-
- dynamicgraph::Matrix Mcreuse( r,parc ); parc=0;
- for( std::list< unsigned int >::iterator iter=nonzeros.begin();
- iter!=nonzeros.end();++iter )
- {
- for( unsigned int i=0;i<r;++i ){ Mcreuse(i,parc) = M(i,*iter);}
- parc++;
- }
-
- dynamicgraph::Matrix Mcreuseinv( Mcreuse.cols(),r );
- START_CHRONO(inv);
- for( int ib=0;ib<BENCH;++ib )
- {
- Eigen::pseudoInverse( Mcreuse, Mcreuseinv );
- }
- STOP_CHRONO(inv,"M+creuseseule");
-
- parc=0;
- 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);
- parc++;
- }
-
- {
- // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
- // sotDEBUG(15) << dynamicgraph::MATLAB <<"Mcreuse = "<< Mcreuse <<endl;
- // sotDEBUG(15) << dynamicgraph::MATLAB <<"Minvnc = "<< Minv <<endl;
- }
+ if (!ib) {
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"Mcreuse = "<< Mcreuse
+ //<<endl; sotDEBUG(15) << dynamicgraph::MATLAB <<"Minvnc = "<< Minv
+ //<<endl;
+ }
+ }
+ STOP_CHRONO(inv, "M+creuse");
+ // 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);
+ if (sumsq > 1e-6) {
+ nonzeros.push_back(j);
+ parc++;
+ }
+ }
+ dynamicgraph::Matrix Mcreuse(r, parc);
+ parc = 0;
+ for (std::list<unsigned int>::iterator iter = nonzeros.begin();
+ iter != nonzeros.end(); ++iter) {
+ for (unsigned int i = 0; i < r; ++i) {
+ Mcreuse(i, parc) = M(i, *iter);
+ }
+ parc++;
+ }
+ dynamicgraph::Matrix Mcreuseinv(Mcreuse.cols(), r);
+ START_CHRONO(inv);
+ for (int ib = 0; ib < BENCH; ++ib) {
+ Eigen::pseudoInverse(Mcreuse, Mcreuseinv);
+ }
+ STOP_CHRONO(inv, "M+creuseseule");
+
+ parc = 0;
+ 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);
+ parc++;
}
+ {
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"M = "<< M <<endl;
+ // sotDEBUG(15) << dynamicgraph::MATLAB <<"Mcreuse = "<< Mcreuse
+ //<<endl; sotDEBUG(15) << dynamicgraph::MATLAB <<"Minvnc = "<< Minv
+ //<<endl;
+ }
+ }
return 0;
}
diff --git a/unitTesting/tools/test_control_pd.cpp b/unitTesting/tools/test_control_pd.cpp
index 834d1b22..ef26fb6f 100644
--- a/unitTesting/tools/test_control_pd.cpp
+++ b/unitTesting/tools/test_control_pd.cpp
@@ -9,23 +9,21 @@
#include <iostream>
#include <sot/core/debug.hh>
-
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
#include <sot/core/control-pd.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-BOOST_AUTO_TEST_CASE(control_pd)
-{
+BOOST_AUTO_TEST_CASE(control_pd) {
sot::ControlPD *aControlPD = ControlPD("acontrol_pd");
aControlPD->init(0.001);
aControlPD->setsize(5);
@@ -45,7 +43,4 @@ BOOST_AUTO_TEST_CASE(control_pd)
aControlPD->recompute(0);
output_test_stream output;
aControlPD->controlSOUT.get(output);
-
}
-
-
diff --git a/unitTesting/tools/test_device.cpp b/unitTesting/tools/test_device.cpp
index 412041ad..ebe3437a 100644
--- a/unitTesting/tools/test_device.cpp
+++ b/unitTesting/tools/test_device.cpp
@@ -34,10 +34,8 @@ public:
~TestDevice() {}
};
-
BOOST_AUTO_TEST_CASE(test_device) {
-
TestDevice aDevice(std::string("simple_humanoid"));
/// Fix constant vector for the control entry in position
@@ -52,15 +50,15 @@ BOOST_AUTO_TEST_CASE(test_device) {
// Specify lower velocity bound
aLowerVelBound[i] = -3.14;
// Specify lower position bound
- aLowerBound[i]=-3.14;
+ aLowerBound[i] = -3.14;
// Specify state vector
aStateVector[i] = 0.1;
// Specify upper velocity bound
anUpperVelBound[i] = 3.14;
// Specify upper position bound
- anUpperBound[i]=3.14;
+ anUpperBound[i] = 3.14;
// Specify control vector
- aControlVector(i)= 0.1;
+ aControlVector(i) = 0.1;
}
dg::Vector expected = aStateVector; // backup initial state vector
@@ -68,7 +66,7 @@ BOOST_AUTO_TEST_CASE(test_device) {
/// Specify state size
aDevice.setStateSize(38);
/// Specify state bounds
- aDevice.setPositionBounds(aLowerBound,anUpperBound);
+ aDevice.setPositionBounds(aLowerBound, anUpperBound);
/// Specify velocity size
aDevice.setVelocitySize(38);
/// Specify velocity
@@ -84,14 +82,12 @@ BOOST_AUTO_TEST_CASE(test_device) {
const unsigned int N = 2000;
for (unsigned int i = 0; i < N; i++) {
aDevice.increment(dt);
- if (i == 0)
- {
+ if (i == 0) {
aDevice.stateSOUT.get(std::cout);
std::ostringstream anoss;
aDevice.stateSOUT.get(anoss);
}
- if (i == 1)
- {
+ if (i == 1) {
aDevice.stateSOUT.get(std::cout);
std::ostringstream anoss;
aDevice.stateSOUT.get(anoss);
@@ -106,19 +102,22 @@ BOOST_AUTO_TEST_CASE(test_device) {
Eigen::Matrix<double, 7, 1> qin, qout;
qin.head<3>() = expected.head<3>();
- Eigen::QuaternionMapd quat (qin.tail<4>().data());
- quat = Eigen::AngleAxisd(expected(5), Eigen::Vector3d::UnitZ())
- * Eigen::AngleAxisd(expected(4), Eigen::Vector3d::UnitY())
- * Eigen::AngleAxisd(expected(3), Eigen::Vector3d::UnitX());
+ Eigen::QuaternionMapd quat(qin.tail<4>().data());
+ quat = Eigen::AngleAxisd(expected(5), Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd(expected(4), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(expected(3), Eigen::Vector3d::UnitX());
- const double T = dt*N;
- Eigen::Matrix<double, 6, 1> control = aControlVector.head<6>()*T;
- SE3().integrate (qin, control, qout);
+ const double T = dt * N;
+ Eigen::Matrix<double, 6, 1> control = aControlVector.head<6>() * T;
+ SE3().integrate(qin, control, qout);
// Manual integration
expected.head<3>() = qout.head<3>();
- expected.segment<3>(3) = Eigen::QuaternionMapd(qout.tail<4>().data()).toRotationMatrix().eulerAngles(2,1,0).reverse();
- for(int i=6; i<expected.size(); i++)
+ expected.segment<3>(3) = Eigen::QuaternionMapd(qout.tail<4>().data())
+ .toRotationMatrix()
+ .eulerAngles(2, 1, 0)
+ .reverse();
+ for (int i = 6; i < expected.size(); i++)
expected[i] = 0.3;
std::cout << expected.transpose() << std::endl;
diff --git a/unitTesting/tools/test_mailbox.cpp b/unitTesting/tools/test_mailbox.cpp
index 5973a4cc..256c51ea 100644
--- a/unitTesting/tools/test_mailbox.cpp
+++ b/unitTesting/tools/test_mailbox.cpp
@@ -10,15 +10,14 @@
#include <iostream>
#include <sot/core/debug.hh>
-
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
-#include <dynamic-graph/factory.h>
#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
#include <sot/core/feature-abstract.hh>
#include <sot/core/mailbox-vector.hh>
#include <sstream>
@@ -28,32 +27,29 @@ using namespace dynamicgraph::sot;
#include <boost/thread.hpp>
-sot::MailboxVector* mailbox = NULL;
+sot::MailboxVector *mailbox = NULL;
-void f( void )
-{
+void f(void) {
Vector vect(25);
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;
- mailbox->post( vect );
- Vector V = mailbox->getObject( vect2, 1 );
- std::cout << vect2 << std::endl;
- std::cout << " getClassName " << mailbox->getClassName() << std::endl;
- std::cout << " getName " << mailbox->getName() << std::endl;
- std::cout << " hasBeenUpdated " << mailbox->hasBeenUpdated() << std::endl;
- std::cout << std::endl;
- }
+ 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;
+ mailbox->post(vect);
+ Vector V = mailbox->getObject(vect2, 1);
+ std::cout << vect2 << std::endl;
+ std::cout << " getClassName " << mailbox->getClassName() << std::endl;
+ std::cout << " getName " << mailbox->getName() << std::endl;
+ std::cout << " hasBeenUpdated " << mailbox->hasBeenUpdated() << std::endl;
+ std::cout << std::endl;
+ }
}
-
-int main( int ,char** )
-{
+int main(int, char **) {
mailbox = new sot::MailboxVector("mail");
- boost::thread th( f );
+ boost::thread th(f);
th.join();
return 0;
diff --git a/unitTesting/tools/test_matrix.cpp b/unitTesting/tools/test_matrix.cpp
index 03cc399f..bcfefb9a 100644
--- a/unitTesting/tools/test_matrix.cpp
+++ b/unitTesting/tools/test_matrix.cpp
@@ -11,9 +11,9 @@
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
-#include <sot/core/feature-abstract.hh>
-#include <sot/core/debug.hh>
#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/debug.hh>
+#include <sot/core/feature-abstract.hh>
#include <iostream>
using namespace std;
@@ -26,43 +26,42 @@ 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. );\
- cout << "dt: "<< dt
-
+#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** )
-{
+int main(int, char **) {
sotDEBUGIN(15);
- struct timeval t0,t1; double dt;
+ struct timeval t0, t1;
+ double dt;
- dynamicgraph::Matrix P(40,40);
- 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 i=0;i<J.rows();++i )
- for( int j=0;j<J.cols();++j ) J(i,j) = (rand()+1.) / RAND_MAX;
+ dynamicgraph::Matrix P(40, 40);
+ 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 i = 0; i < J.rows(); ++i)
+ for (int j = 0; j < J.cols(); ++j)
+ J(i, j) = (rand() + 1.) / RAND_MAX;
int nbIter = 100000;
- dt=0;
- gettimeofday(&t0,NULL);
- for( int iter=0;iter<nbIter;++iter )
- {
- gettimeofday(&t0,NULL);
- //J.multiply(P,JK);
- //prod(J.matrix,P.matrix,JK.matrix);
- JK = J*P;
- gettimeofday(&t1,NULL);
- dt += ( (double)(t1.tv_sec-t0.tv_sec)
- + (double)(t1.tv_usec-t0.tv_usec) / 1000. / 1000. );
-
- }
- //sotCHRONO1 <<endl;
- cout<<dt/nbIter<<endl;
+ dt = 0;
+ gettimeofday(&t0, NULL);
+ for (int iter = 0; iter < nbIter; ++iter) {
+ gettimeofday(&t0, NULL);
+ // J.multiply(P,JK);
+ // prod(J.matrix,P.matrix,JK.matrix);
+ JK = J * P;
+ gettimeofday(&t1, NULL);
+ dt += ((double)(t1.tv_sec - t0.tv_sec) +
+ (double)(t1.tv_usec - t0.tv_usec) / 1000. / 1000.);
+ }
+ // sotCHRONO1 <<endl;
+ cout << dt / nbIter << endl;
sotDEBUGOUT(15);
return 0;
diff --git a/unitTesting/tools/test_robot_utils.cpp b/unitTesting/tools/test_robot_utils.cpp
index 959bdfeb..6f195510 100644
--- a/unitTesting/tools/test_robot_utils.cpp
+++ b/unitTesting/tools/test_robot_utils.cpp
@@ -9,10 +9,10 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
+#include <dynamic-graph/factory.h>
#include <iostream>
-#include <sot/core/robot-utils.hh>
#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
+#include <sot/core/robot-utils.hh>
using namespace std;
using namespace dynamicgraph;
@@ -20,44 +20,35 @@ using namespace dynamicgraph::sot;
std::string localName("robot_test");
RobotUtilShrPtr robot_util;
-int main( void )
-{
+int main(void) {
robot_util = createRobotUtil(localName);
/*Test set and get joint_limits_for_id */
const double upper_lim(1);
const double lower_lim(2);
- robot_util->set_joint_limits_for_id(1,lower_lim,upper_lim);
+ robot_util->set_joint_limits_for_id(1, lower_lim, upper_lim);
if (robot_util->get_joint_limits_from_id(1).upper == upper_lim &&
- robot_util->get_joint_limits_from_id(1).lower == lower_lim)
- {
- std::cout << "joint_limits_for_id works !" << std::endl; }
- else
- {
- std::cout << "ERROR: joint_limits_for_id does not work !" << std::endl;
+ robot_util->get_joint_limits_from_id(1).lower == lower_lim) {
+ std::cout << "joint_limits_for_id works !" << std::endl;
+ } else {
+ std::cout << "ERROR: joint_limits_for_id does not work !" << std::endl;
}
if (robot_util->cp_get_joint_limits_from_id(1).upper == upper_lim &&
- robot_util->cp_get_joint_limits_from_id(1).lower == lower_lim)
- {
- std::cout << "cp_get_joint_limits_for_id works !" << std::endl; }
- else
- {
- std::cout << "ERROR: cp_get_joint_limits_for_id does not work !"
- << std::endl;
+ robot_util->cp_get_joint_limits_from_id(1).lower == lower_lim) {
+ std::cout << "cp_get_joint_limits_for_id works !" << std::endl;
+ } else {
+ std::cout << "ERROR: cp_get_joint_limits_for_id does not work !"
+ << std::endl;
}
-
/*Test set and get name_to_id */
const std::string joint_name("test_joint");
const Index joint_id(10);
- robot_util->set_name_to_id(joint_name,joint_id);
+ robot_util->set_name_to_id(joint_name, joint_id);
if (robot_util->get_id_from_name(joint_name) == joint_id &&
- robot_util->get_name_from_id(joint_id) == joint_name)
- {
- std::cout << "name_to_id works !" << std::endl;
- }
- else
- {
- std::cout << "ERROR: name_to_id does not work !" << std::endl;
+ robot_util->get_name_from_id(joint_id) == joint_name) {
+ std::cout << "name_to_id works !" << std::endl;
+ } else {
+ std::cout << "ERROR: name_to_id does not work !" << std::endl;
}
/*Test create_id_to_name_map */
@@ -66,32 +57,27 @@ int main( void )
/*Test set urdf_to_sot */
dg::Vector urdf_to_sot(3);
- urdf_to_sot << 0,2,1;
+ urdf_to_sot << 0, 2, 1;
robot_util->set_urdf_to_sot(urdf_to_sot);
- if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot )
- {
- std::cout << "urdf_to_sot works !" << std::endl;
- }
- else
- {
- std::cout << "ERROR: urdf_to_sot does not work !" << std::endl;
+ if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot) {
+ std::cout << "urdf_to_sot works !" << std::endl;
+ } else {
+ std::cout << "ERROR: urdf_to_sot does not work !" << std::endl;
}
/*Test joints_urdf_to_sot and joints_sot_to_urdf */
dg::Vector q_urdf(3);
- q_urdf << 10,20,30;
+ q_urdf << 10, 20, 30;
dg::Vector q_sot(3);
dg::Vector q_test_urdf(3);
robot_util->joints_urdf_to_sot(q_urdf, q_sot);
robot_util->joints_sot_to_urdf(q_sot, q_test_urdf);
- if (q_urdf == q_test_urdf )
- {
- std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !"
- << std::endl;
- }
- else
- {
- std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf "
- "do not work !" << std::endl;
+ if (q_urdf == q_test_urdf) {
+ std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !"
+ << std::endl;
+ } else {
+ std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf "
+ "do not work !"
+ << std::endl;
}
/*Test velocity_sot_to_urdf and velocity_urdf_to_sot */
@@ -112,12 +98,12 @@ int main( void )
/*Test config_urdf_to_sot and config_sot_to_urdf */
dg::Vector q2_sot(9);
- robot_util->config_urdf_to_sot(q2_urdf,q2_sot);
- robot_util->config_sot_to_urdf(q2_sot,q2_urdf);
+ robot_util->config_urdf_to_sot(q2_urdf, q2_sot);
+ robot_util->config_sot_to_urdf(q2_sot, q2_urdf);
std::cout << "config_urdf_to_sot and config_sot_to_urdf work !" << std::endl;
robot_util->display(std::cout);
- robot_util->sendMsg("test",MSG_TYPE_ERROR_STREAM);
+ robot_util->sendMsg("test", MSG_TYPE_ERROR_STREAM);
/*Test set_name_to_force_id of forceutil */
const std::string rf("rf");
@@ -125,49 +111,41 @@ int main( void )
const std::string lh("lh");
const std::string rh("rh");
- robot_util->m_force_util.set_name_to_force_id(rf,0);
- robot_util->m_force_util.set_name_to_force_id(lf,1);
- robot_util->m_force_util.set_name_to_force_id(lh,2);
- robot_util->m_force_util.set_name_to_force_id(rh,3);
+ robot_util->m_force_util.set_name_to_force_id(rf, 0);
+ robot_util->m_force_util.set_name_to_force_id(lf, 1);
+ robot_util->m_force_util.set_name_to_force_id(lh, 2);
+ robot_util->m_force_util.set_name_to_force_id(rh, 3);
dg::Vector lf_lim(6);
- lf_lim << 1,2,3,4,5,6;
+ lf_lim << 1, 2, 3, 4, 5, 6;
dg::Vector uf_lim(6);
- uf_lim << 10,20,30,40,50,60;
+ uf_lim << 10, 20, 30, 40, 50, 60;
robot_util->m_force_util.set_force_id_to_limits(1, lf_lim, uf_lim);
- if( robot_util->m_force_util.get_id_from_name(rf) == 0 &&
+ if (robot_util->m_force_util.get_id_from_name(rf) == 0 &&
robot_util->m_force_util.get_id_from_name(lf) == 1 &&
robot_util->m_force_util.get_id_from_name(lh) == 2 &&
- robot_util->m_force_util.get_id_from_name(rh) == 3)
- {
- std::cout << "force_util set and get id_from_name work !" << std::endl;
- }
- else
- {
- std::cout << "ERROR: force_util set and get id_from_name do not work !"
- << std::endl;
+ robot_util->m_force_util.get_id_from_name(rh) == 3) {
+ std::cout << "force_util set and get id_from_name work !" << std::endl;
+ } else {
+ std::cout << "ERROR: force_util set and get id_from_name do not work !"
+ << std::endl;
}
- if( robot_util->m_force_util.get_name_from_id(0) == rf &&
+ if (robot_util->m_force_util.get_name_from_id(0) == rf &&
robot_util->m_force_util.get_name_from_id(1) == lf &&
robot_util->m_force_util.get_name_from_id(2) == lh &&
- robot_util->m_force_util.get_name_from_id(3) == rh)
- {
- std::cout << "force_util get name_from_id works !" << std::endl;
- }
- else
- {
- std::cout << "ERROR: force_util get name_from_id does not work !"
- << std::endl;
+ robot_util->m_force_util.get_name_from_id(3) == rh) {
+ std::cout << "force_util get name_from_id works !" << std::endl;
+ } else {
+ std::cout << "ERROR: force_util get name_from_id does not work !"
+ << std::endl;
}
if (robot_util->m_force_util.get_limits_from_id(1).upper == uf_lim &&
- robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim)
- {
- std::cout << "force_util set and get id to limits work !" << std::endl;
- }
- else
- {
+ robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim) {
+ std::cout << "force_util set and get id to limits work !" << std::endl;
+ } else {
std::cout << "ERROR: force_util set and get id to "
- "limits works do not work !" << std::endl;
+ "limits works do not work !"
+ << std::endl;
}
robot_util->m_force_util.display(std::cout);
robot_util->m_foot_util.display(std::cout);
diff --git a/unitTesting/traces/files.cpp b/unitTesting/traces/files.cpp
index 98d437f3..9e6bb176 100644
--- a/unitTesting/traces/files.cpp
+++ b/unitTesting/traces/files.cpp
@@ -10,22 +10,19 @@
#include <iostream>
#include <sot/core/debug.hh>
-#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
+#include <boost/filesystem/path.hpp>
using namespace std;
-int main()
-{
-
- boost::filesystem::create_directory( "foobar" );
- ofstream file( "foobar/cheeze" );
- file << "tastes good!\n";
- file.close();
- if ( !boost::filesystem::exists( "foobar/cheeze" ) )
- std::cout << "Something is rotten in foobar\n";
-
+int main() {
+ boost::filesystem::create_directory("foobar");
+ ofstream file("foobar/cheeze");
+ file << "tastes good!\n";
+ file.close();
+ if (!boost::filesystem::exists("foobar/cheeze"))
+ std::cout << "Something is rotten in foobar\n";
return 0;
}
diff --git a/unitTesting/traces/test_traces.cpp b/unitTesting/traces/test_traces.cpp
index cd68ecc8..74edac2b 100644
--- a/unitTesting/traces/test_traces.cpp
+++ b/unitTesting/traces/test_traces.cpp
@@ -9,38 +9,46 @@
#include <iostream>
-#include <sot/core/debug.hh>
+#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-base.h>
-#include <dynamic-graph/signal.h>
#include <dynamic-graph/signal-time-dependent.h>
+#include <dynamic-graph/signal.h>
#include <dynamic-graph/tracer.h>
-#include <dynamic-graph/linear-algebra.h>
+#include <sot/core/debug.hh>
using namespace std;
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
-double& f( double& res,const int& /*t*/ ) { cout << "SIGM!"<<endl; return res; }
+double &f(double &res, const int & /*t*/) {
+ cout << "SIGM!" << endl;
+ return res;
+}
-int main()
-{
- Signal<Vector,int> sig1( "sig1" );
- Vector v1(2); v1.fill(1.1); sig1 = v1;
+int main() {
+ Signal<Vector, int> sig1("sig1");
+ Vector v1(2);
+ v1.fill(1.1);
+ sig1 = v1;
- Signal<Vector,int> sig2( "sig2" );
- Vector v2(4); v2.fill(2.); sig2 = v2;
+ Signal<Vector, int> sig2("sig2");
+ Vector v2(4);
+ v2.fill(2.);
+ sig2 = v2;
- Signal<Vector,int> sig3( "sig3" );
- Vector v3(6); v3.fill(3.); sig3 = v3;
+ Signal<Vector, int> sig3("sig3");
+ Vector v3(6);
+ v3.fill(3.);
+ sig3 = v3;
- SignalTimeDependent<double,int> sigM( f,sotNOSIGNAL,"sigM" );
+ SignalTimeDependent<double, int> sigM(f, sotNOSIGNAL, "sigM");
sigM.access(0);
- Tracer* tracer = new Tracer( "trace" );
- tracer->addSignalToTrace( sig1 );
- tracer->openFiles( "/tmp/sot-core","tr_",".dat" );
+ Tracer *tracer = new Tracer("trace");
+ tracer->addSignalToTrace(sig1);
+ tracer->openFiles("/tmp/sot-core", "tr_", ".dat");
- tracer->addSignalToTrace( sig2 );
+ tracer->addSignalToTrace(sig2);
return 0;
}
--
GitLab