Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-dynamic-pinocchio
Commits
290b0d23
Commit
290b0d23
authored
Jan 13, 2016
by
Rohan Budhiraja
Browse files
[eigen] replace jrl-mal with eigen, unless needed by jrl-dynamics.
parent
b143f397
Changes
29
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
290b0d23
...
...
@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE
(
cmake/base.cmake
)
INCLUDE
(
cmake/boost.cmake
)
INCLUDE
(
cmake/eigen.cmake
)
INCLUDE
(
cmake/lapack.cmake
)
INCLUDE
(
cmake/cpack.cmake
)
...
...
@@ -39,12 +40,10 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
SETUP_PROJECT
()
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY
(
"jrl-mal >= 1.9.0"
)
ADD_REQUIRED_DEPENDENCY
(
"jrl-dynamics >= 1.19.0"
)
ADD_REQUIRED_DEPENDENCY
(
"dynamic-graph >=
2.5
.0"
)
ADD_REQUIRED_DEPENDENCY
(
"sot-core >=
2.5
"
)
ADD_REQUIRED_DEPENDENCY
(
"dynamic-graph >=
3.0
.0"
)
ADD_REQUIRED_DEPENDENCY
(
"sot-core >=
3.0.0
"
)
ADD_REQUIRED_DEPENDENCY
(
"sot-tools"
)
# List plug-ins that will be compiled.
...
...
@@ -73,6 +72,7 @@ PKG_CONFIG_APPEND_LIBS(${LIBRARY_NAME})
# Boost
SET
(
BOOST_COMPONENTS filesystem system
)
SEARCH_FOR_BOOST
()
SEARCH_FOR_EIGEN
()
# Add subdirectories.
ADD_SUBDIRECTORY
(
include
)
...
...
README.md
View file @
290b0d23
...
...
@@ -29,9 +29,8 @@ have to be available on your machine.
-
Libraries:
-
[
jrl-dynamics
][
jrl-dynamics
]
(>= 1.16.1)
-
[
dynamic-graph
][
dynamic-graph
]
(>= 1.0.0)
-
[
sot-core
][
sot-core
]
(>= 1.0.0)
-
[
jrl-mal
][
jrl-mal
]
(>= 1.8.0)
-
[
dynamic-graph
][
dynamic-graph
]
(>= 3.0.0)
-
[
sot-core
][
sot-core
]
(>= 3.0.0)
-
Closed source libraries:
-
hrp2Dynamics (>= 1.3.0)
-
hrp2-10-optimized (>= 1.3.0) [optional]
...
...
@@ -43,5 +42,4 @@ have to be available on your machine.
[
dynamic-graph
]:
http://github.com/stack-of-tasks/dynamic-graph
[
jrl-dynamics
]:
http://github.com/jrl-umi3218/jrl-dynamics
[
jrl-mal
]:
http://github.com/jrl-umi3218/jrl-mal
[
sot-core
]:
http://github.com/stack-of-tasks/sot-core
debian/control
View file @
290b0d23
...
...
@@ -4,7 +4,6 @@ Maintainer: Thomas Moulard <thomas.moulard@gmail.com>
Build-Depends: debhelper (>= 7.0.50~), cmake (>= 2.6),
doxygen (>= 1.6.3),
pkg-config (>= 0.22),
libjrl-mal-dev (>= 1.9.0.99),
libjrl-dynamics-dev (>= 1.19.1),
libdynamic-graph-dev (>= 1.0.0.99),
libsot-core-dev (>= 1.0.0.99),
...
...
include/sot-dynamic/angle-estimator.h
View file @
290b0d23
...
...
@@ -39,16 +39,15 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include
<
jrl/mal/boost.h
h>
namespace
ml
=
maal
::
boost
;
#include
<
dynamic-graph/linear-algebra.
h>
/* SOT */
#include
<dynamic-graph/entity.h>
#include
<dynamic-graph/signal-ptr.h>
#include
<dynamic-graph/signal-time-dependent.h>
#include
<sot/core/matrix-homogeneous.hh>
#include
<sot/core/vector-roll-pitch-yaw.hh>
#include
<sot/core/matrix-rotation.hh>
#include
<sot/core/matrix-geometry.hh>
/* STD */
#include
<string>
...
...
@@ -79,21 +78,21 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
dg
::
SignalPtr
<
MatrixHomogeneous
,
int
>
sensorEmbeddedPositionSIN
;
// waistRchest
dg
::
SignalPtr
<
MatrixHomogeneous
,
int
>
contactWorldPositionSIN
;
// estimate(worldRf)
dg
::
SignalPtr
<
MatrixHomogeneous
,
int
>
contactEmbeddedPositionSIN
;
// waistRleg
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
anglesSOUT
;
// [ flex1 flex2 yaw_drift ]
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
anglesSOUT
;
// [ flex1 flex2 yaw_drift ]
dg
::
SignalTimeDependent
<
MatrixRotation
,
int
>
flexibilitySOUT
;
// footRleg
dg
::
SignalTimeDependent
<
MatrixRotation
,
int
>
driftSOUT
;
// Ryaw = worldRc est(wRc)^-1
dg
::
SignalTimeDependent
<
MatrixRotation
,
int
>
sensorWorldRotationSOUT
;
// worldRc
dg
::
SignalTimeDependent
<
MatrixRotation
,
int
>
waistWorldRotationSOUT
;
// worldRwaist
dg
::
SignalTimeDependent
<
MatrixHomogeneous
,
int
>
waistWorldPositionSOUT
;
// worldMwaist
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
waistWorldPoseRPYSOUT
;
// worldMwaist
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
waistWorldPoseRPYSOUT
;
// worldMwaist
dg
::
SignalPtr
<
ml
::
Matrix
,
int
>
jacobianSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
qdotSIN
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
xff_dotSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
qdotSOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
jacobianSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
qdotSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
xff_dotSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
qdotSOUT
;
public:
/* --- FUNCTIONS --- */
ml
::
Vector
&
computeAngles
(
ml
::
Vector
&
res
,
dynamicgraph
::
Vector
&
computeAngles
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
MatrixRotation
&
computeFlexibilityFromAngles
(
MatrixRotation
&
res
,
const
int
&
time
);
...
...
@@ -105,11 +104,11 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
const
int
&
time
);
MatrixHomogeneous
&
computeWaistWorldPosition
(
MatrixHomogeneous
&
res
,
const
int
&
time
);
ml
::
Vector
&
computeWaistWorldPoseRPY
(
ml
::
Vector
&
res
,
dynamicgraph
::
Vector
&
computeWaistWorldPoseRPY
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
compute_xff_dotSOUT
(
ml
::
Vector
&
res
,
dynamicgraph
::
Vector
&
compute_xff_dotSOUT
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
compute_qdotSOUT
(
ml
::
Vector
&
res
,
dynamicgraph
::
Vector
&
compute_qdotSOUT
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
public:
/* --- PARAMS --- */
...
...
include/sot-dynamic/dynamic.h
View file @
290b0d23
...
...
@@ -30,9 +30,8 @@
#include
<map>
/* Matrix */
#include
<jrl/mal/boost.hh>
#include
"jrl/mal/matrixabstractlayer.hh"
namespace
ml
=
maal
::
boost
;
#include
<dynamic-graph/linear-algebra.h>
/* JRL dynamic */
#include
<abstract-robot-dynamics/humanoid-dynamic-robot.hh>
...
...
@@ -47,7 +46,7 @@ namespace djj = dynamicsJRLJapan;
#include
<dynamic-graph/signal-ptr.h>
#include
<dynamic-graph/signal-time-dependent.h>
#include
<sot/core/exception-dynamic.hh>
#include
<sot/core/matrix-
homogeneous
.hh>
#include
<sot/core/matrix-
geometry
.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
...
...
@@ -146,10 +145,10 @@ class SOTDYNAMIC_EXPORT Dynamic
void
parseConfigFiles
(
void
);
public:
/* --- SIGNAL ACTIVATION --- */
dg
::
SignalTimeDependent
<
ml
::
Matrix
,
int
>
&
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>
&
createEndeffJacobianSignal
(
const
std
::
string
&
signame
,
CjrlJoint
*
inJoint
);
dg
::
SignalTimeDependent
<
ml
::
Matrix
,
int
>
&
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>
&
createJacobianSignal
(
const
std
::
string
&
signame
,
CjrlJoint
*
inJoint
);
void
destroyJacobianSignal
(
const
std
::
string
&
signame
);
...
...
@@ -157,11 +156,11 @@ class SOTDYNAMIC_EXPORT Dynamic
createPositionSignal
(
const
std
::
string
&
signame
,
CjrlJoint
*
inJoint
);
void
destroyPositionSignal
(
const
std
::
string
&
signame
);
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>&
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>&
createVelocitySignal
(
const
std
::
string
&
signame
,
CjrlJoint
*
inJoint
);
void
destroyVelocitySignal
(
const
std
::
string
&
signame
);
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>&
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>&
createAccelerationSignal
(
const
std
::
string
&
signame
,
CjrlJoint
*
inJoint
);
void
destroyAccelerationSignal
(
const
std
::
string
&
signame
);
...
...
@@ -177,12 +176,12 @@ class SOTDYNAMIC_EXPORT Dynamic
public:
/* --- SIGNAL --- */
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
jointPositionSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
freeFlyerPositionSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
jointVelocitySIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
freeFlyerVelocitySIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
jointAccelerationSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
freeFlyerAccelerationSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
jointPositionSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
freeFlyerPositionSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
jointVelocitySIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
freeFlyerVelocitySIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
jointAccelerationSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
freeFlyerAccelerationSIN
;
// protected:
public:
...
...
@@ -194,57 +193,57 @@ class SOTDYNAMIC_EXPORT Dynamic
int
&
initNewtonEuler
(
int
&
dummy
,
int
time
);
public:
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
zmpSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Matrix
,
int
>
JcomSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
comSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Matrix
,
int
>
inertiaSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
zmpSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>
JcomSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
comSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>
inertiaSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Matrix
,
int
>&
jacobiansSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>&
jacobiansSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
MatrixHomogeneous
,
int
>&
positionsSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>&
velocitiesSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>&
accelerationsSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>&
velocitiesSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>&
accelerationsSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
double
,
int
>
footHeightSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
upperJlSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
lowerJlSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
upperVlSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
lowerVlSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
upperTlSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
lowerTlSOUT
;
dg
::
Signal
<
ml
::
Vector
,
int
>
inertiaRotorSOUT
;
dg
::
Signal
<
ml
::
Vector
,
int
>
gearRatioSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Matrix
,
int
>
inertiaRealSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
MomentaSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
AngularMomentumSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
dynamicDriftSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
upperJlSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
lowerJlSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
upperVlSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
lowerVlSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
upperTlSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
lowerTlSOUT
;
dg
::
Signal
<
dynamicgraph
::
Vector
,
int
>
inertiaRotorSOUT
;
dg
::
Signal
<
dynamicgraph
::
Vector
,
int
>
gearRatioSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>
inertiaRealSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
MomentaSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
AngularMomentumSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
dynamicDriftSOUT
;
protected:
ml
::
Vector
&
computeZmp
(
ml
::
Vector
&
res
,
int
time
);
ml
::
Vector
&
computeMomenta
(
ml
::
Vector
&
res
,
int
time
);
ml
::
Vector
&
computeAngularMomentum
(
ml
::
Vector
&
res
,
int
time
);
ml
::
Matrix
&
computeJcom
(
ml
::
Matrix
&
res
,
int
time
);
ml
::
Vector
&
computeCom
(
ml
::
Vector
&
res
,
int
time
);
ml
::
Matrix
&
computeInertia
(
ml
::
Matrix
&
res
,
int
time
);
ml
::
Matrix
&
computeInertiaReal
(
ml
::
Matrix
&
res
,
int
time
);
dynamicgraph
::
Vector
&
computeZmp
(
dynamicgraph
::
Vector
&
res
,
int
time
);
dynamicgraph
::
Vector
&
computeMomenta
(
dynamicgraph
::
Vector
&
res
,
int
time
);
dynamicgraph
::
Vector
&
computeAngularMomentum
(
dynamicgraph
::
Vector
&
res
,
int
time
);
dynamicgraph
::
Matrix
&
computeJcom
(
dynamicgraph
::
Matrix
&
res
,
int
time
);
dynamicgraph
::
Vector
&
computeCom
(
dynamicgraph
::
Vector
&
res
,
int
time
);
dynamicgraph
::
Matrix
&
computeInertia
(
dynamicgraph
::
Matrix
&
res
,
int
time
);
dynamicgraph
::
Matrix
&
computeInertiaReal
(
dynamicgraph
::
Matrix
&
res
,
int
time
);
double
&
computeFootHeight
(
double
&
res
,
int
time
);
ml
::
Matrix
&
computeGenericJacobian
(
CjrlJoint
*
j
,
ml
::
Matrix
&
res
,
int
time
);
ml
::
Matrix
&
computeGenericEndeffJacobian
(
CjrlJoint
*
j
,
ml
::
Matrix
&
res
,
int
time
);
dynamicgraph
::
Matrix
&
computeGenericJacobian
(
CjrlJoint
*
j
,
dynamicgraph
::
Matrix
&
res
,
int
time
);
dynamicgraph
::
Matrix
&
computeGenericEndeffJacobian
(
CjrlJoint
*
j
,
dynamicgraph
::
Matrix
&
res
,
int
time
);
MatrixHomogeneous
&
computeGenericPosition
(
CjrlJoint
*
j
,
MatrixHomogeneous
&
res
,
int
time
);
ml
::
Vector
&
computeGenericVelocity
(
CjrlJoint
*
j
,
ml
::
Vector
&
res
,
int
time
);
ml
::
Vector
&
computeGenericAcceleration
(
CjrlJoint
*
j
,
ml
::
Vector
&
res
,
int
time
);
dynamicgraph
::
Vector
&
computeGenericVelocity
(
CjrlJoint
*
j
,
dynamicgraph
::
Vector
&
res
,
int
time
);
dynamicgraph
::
Vector
&
computeGenericAcceleration
(
CjrlJoint
*
j
,
dynamicgraph
::
Vector
&
res
,
int
time
);
ml
::
Vector
&
getUpperJointLimits
(
ml
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
getLowerJointLimits
(
ml
::
Vector
&
res
,
const
int
&
time
);
dynamicgraph
::
Vector
&
getUpperJointLimits
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
dynamicgraph
::
Vector
&
getLowerJointLimits
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
getUpperVelocityLimits
(
ml
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
getLowerVelocityLimits
(
ml
::
Vector
&
res
,
const
int
&
time
);
dynamicgraph
::
Vector
&
getUpperVelocityLimits
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
dynamicgraph
::
Vector
&
getLowerVelocityLimits
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
getUpperTorqueLimits
(
ml
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
getLowerTorqueLimits
(
ml
::
Vector
&
res
,
const
int
&
time
);
dynamicgraph
::
Vector
&
getUpperTorqueLimits
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
dynamicgraph
::
Vector
&
getLowerTorqueLimits
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
ml
::
Vector
&
computeTorqueDrift
(
ml
::
Vector
&
res
,
const
int
&
time
);
dynamicgraph
::
Vector
&
computeTorqueDrift
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
public:
/* --- PARAMS --- */
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
...
...
@@ -272,7 +271,7 @@ class SOTDYNAMIC_EXPORT Dynamic
/// commands. An empty CjrlBody is also created and attached to the joint.
void
createJoint
(
const
std
::
string
&
inJointName
,
const
std
::
string
&
inJointType
,
const
ml
::
Matrix
&
inPosition
);
const
dynamicgraph
::
Matrix
&
inPosition
);
/// \brief Set a joint as root joint of the robot.
void
setRootJoint
(
const
std
::
string
&
inJointName
);
...
...
@@ -302,14 +301,14 @@ class SOTDYNAMIC_EXPORT Dynamic
///
/// \param inJointName name of the joint to which the body is attached,
/// \param inCom local center of mass.
void
setLocalCenterOfMass
(
const
std
::
string
&
inJointName
,
ml
::
Vector
inCom
);
void
setLocalCenterOfMass
(
const
std
::
string
&
inJointName
,
dynamicgraph
::
Vector
inCom
);
/// \brief Set inertia matrix of a body
///
/// \param inJointName name of the joint to which the body is attached,
/// \param inMatrix inertia matrix.
void
setInertiaMatrix
(
const
std
::
string
&
inJointName
,
ml
::
Matrix
inMatrix
);
dynamicgraph
::
Matrix
inMatrix
);
/// \brief Set specific joints
///
...
...
@@ -325,10 +324,10 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \param inThumbAxis thumb axis in wrist local frame,
/// \param inForefingerAxis forefinger axis in wrist local frame,
/// \param inPalmNormalAxis palm normal in wrist local frame,
void
setHandParameters
(
bool
inRight
,
const
ml
::
Vector
&
inCenter
,
const
ml
::
Vector
&
inThumbAxis
,
const
ml
::
Vector
&
inForefingerAxis
,
const
ml
::
Vector
&
inPalmNormal
);
void
setHandParameters
(
bool
inRight
,
const
dynamicgraph
::
Vector
&
inCenter
,
const
dynamicgraph
::
Vector
&
inThumbAxis
,
const
dynamicgraph
::
Vector
&
inForefingerAxis
,
const
dynamicgraph
::
Vector
&
inPalmNormal
);
/// \brief Set foot parameters
///
...
...
@@ -338,14 +337,14 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \param inAnklePosition ankle position in foot local frame,
void
setFootParameters
(
bool
inRight
,
const
double
&
inSoleLength
,
const
double
&
inSoleWidth
,
const
ml
::
Vector
&
inAnklePosition
);
const
dynamicgraph
::
Vector
&
inAnklePosition
);
/// \brief Set gaze parameters
///
/// \param inGazeOrigin origin of the gaze in gaze joint local frame,
/// \param inGazeDirection direction of the gase in gaze joint local frame.
void
setGazeParameters
(
const
ml
::
Vector
&
inGazeOrigin
,
const
ml
::
Vector
&
inGazeDirection
);
void
setGazeParameters
(
const
dynamicgraph
::
Vector
&
inGazeOrigin
,
const
dynamicgraph
::
Vector
&
inGazeDirection
);
/// \brief Get length of left foot sole.
///
...
...
@@ -360,7 +359,7 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \brief Get left ankle position in foot frame
///
/// The robot is assumed to be symmetric.
ml
::
Vector
getAnklePositionInFootFrame
()
const
;
dynamicgraph
::
Vector
getAnklePositionInFootFrame
()
const
;
/// @}
///
...
...
include/sot-dynamic/force-compensation.h
View file @
290b0d23
...
...
@@ -26,16 +26,15 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include
<
jrl/mal/boost.h
h>
namespace
ml
=
maal
::
boost
;
#include
<
dynamic-graph/linear-algebra.
h>
/* SOT */
#include
<dynamic-graph/entity.h>
#include
<dynamic-graph/signal-ptr.h>
#include
<dynamic-graph/signal-time-dependent.h>
#include
<sot/core/matrix-rotation.hh>
#include
<sot/core/matrix-force.hh>
#include
<sot/core/matrix-homogeneous.hh>
#include
<sot/core/matrix-geometry.hh>
/* STD */
#include
<string>
...
...
@@ -73,53 +72,53 @@ namespace dynamicgraph { namespace sot {
ForceCompensation
(
void
);
static
MatrixForce
&
computeHandXworld
(
const
MatrixRotation
&
worldRhand
,
const
ml
::
Vector
&
transSensorCom
,
const
dynamicgraph
::
Vector
&
transSensorCom
,
MatrixForce
&
res
);
static
MatrixForce
&
computeHandVsensor
(
const
MatrixRotation
&
sensorRhand
,
MatrixForce
&
res
);
static
MatrixForce
&
computeSensorXhand
(
const
MatrixRotation
&
sensorRhand
,
const
ml
::
Vector
&
transSensorCom
,
const
dynamicgraph
::
Vector
&
transSensorCom
,
MatrixForce
&
res
);
/* static
ml
::Matrix& computeInertiaSensor( const
ml
::Matrix& inertiaJoint, */
/* static
dynamicgraph
::Matrix& computeInertiaSensor( const
dynamicgraph
::Matrix& inertiaJoint, */
/* const MatrixForce& sensorXhand, */
/*
ml
::Matrix& res ); */
/*
dynamicgraph
::Matrix& res ); */
static
ml
::
Vector
&
computeTorsorCompensated
(
const
ml
::
Vector
&
torqueInput
,
const
ml
::
Vector
&
torquePrecompensation
,
const
ml
::
Vector
&
gravity
,
static
dynamicgraph
::
Vector
&
computeTorsorCompensated
(
const
dynamicgraph
::
Vector
&
torqueInput
,
const
dynamicgraph
::
Vector
&
torquePrecompensation
,
const
dynamicgraph
::
Vector
&
gravity
,
const
MatrixForce
&
handXworld
,
const
MatrixForce
&
handVsensor
,
const
ml
::
Matrix
&
gainSensor
,
const
ml
::
Vector
&
momentum
,
ml
::
Vector
&
res
);
static
ml
::
Vector
&
crossProduct_V_F
(
const
ml
::
Vector
&
velocity
,
const
ml
::
Vector
&
force
,
ml
::
Vector
&
res
);
static
ml
::
Vector
&
computeMomentum
(
const
ml
::
Vector
&
velocity
,
const
ml
::
Vector
&
acceleration
,
const
dynamicgraph
::
Matrix
&
gainSensor
,
const
dynamicgraph
::
Vector
&
momentum
,
dynamicgraph
::
Vector
&
res
);
static
dynamicgraph
::
Vector
&
crossProduct_V_F
(
const
dynamicgraph
::
Vector
&
velocity
,
const
dynamicgraph
::
Vector
&
force
,
dynamicgraph
::
Vector
&
res
);
static
dynamicgraph
::
Vector
&
computeMomentum
(
const
dynamicgraph
::
Vector
&
velocity
,
const
dynamicgraph
::
Vector
&
acceleration
,
const
MatrixForce
&
sensorXhand
,
const
ml
::
Matrix
&
inertiaJoint
,
ml
::
Vector
&
res
);
const
dynamicgraph
::
Matrix
&
inertiaJoint
,
dynamicgraph
::
Vector
&
res
);
static
ml
::
Vector
&
computeDeadZone
(
const
ml
::
Vector
&
torqueInput
,
const
ml
::
Vector
&
deadZoneLimit
,
ml
::
Vector
&
res
);
static
dynamicgraph
::
Vector
&
computeDeadZone
(
const
dynamicgraph
::
Vector
&
torqueInput
,
const
dynamicgraph
::
Vector
&
deadZoneLimit
,
dynamicgraph
::
Vector
&
res
);
public:
// CALIBRATION
std
::
list
<
ml
::
Vector
>
torsorList
;
std
::
list
<
dynamicgraph
::
Vector
>
torsorList
;
std
::
list
<
MatrixRotation
>
rotationList
;
void
clearCalibration
(
void
);
void
addCalibrationValue
(
const
ml
::
Vector
&
torsor
,
void
addCalibrationValue
(
const
dynamicgraph
::
Vector
&
torsor
,
const
MatrixRotation
&
worldRhand
);
ml
::
Vector
calibrateTransSensorCom
(
const
ml
::
Vector
&
gravity
,
dynamicgraph
::
Vector
calibrateTransSensorCom
(
const
dynamicgraph
::
Vector
&
gravity
,
const
MatrixRotation
&
handRsensor
);
ml
::
Vector
calibrateGravity
(
const
MatrixRotation
&
handRsensor
,
dynamicgraph
::
Vector
calibrateGravity
(
const
MatrixRotation
&
handRsensor
,
bool
precompensationCalibration
=
false
,
const
MatrixRotation
&
hand0Rsensor
=
I3
);
...
...
@@ -148,35 +147,35 @@ namespace dynamicgraph { namespace sot {
public:
/* --- SIGNAL --- */
/* --- INPUTS --- */
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
torsorSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
torsorSIN
;
dg
::
SignalPtr
<
MatrixRotation
,
int
>
worldRhandSIN
;
/* --- CONSTANTS --- */
dg
::
SignalPtr
<
MatrixRotation
,
int
>
handRsensorSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
translationSensorComSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
gravitySIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
precompensationSIN
;
dg
::
SignalPtr
<
ml
::
Matrix
,
int
>
gainSensorSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
deadZoneLimitSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
transSensorJointSIN
;
dg
::
SignalPtr
<
ml
::
Matrix
,
int
>
inertiaJointSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
translationSensorComSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
gravitySIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
precompensationSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
gainSensorSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
deadZoneLimitSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
transSensorJointSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
inertiaJointSIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
velocitySIN
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
accelerationSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
velocitySIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
accelerationSIN
;
/* --- INTERMEDIATE OUTPUTS --- */
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
handXworldSOUT
;
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
handVsensorSOUT
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
torsorDeadZoneSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
torsorDeadZoneSIN
;
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
sensorXhandSOUT
;
//dg::SignalTimeDependent<
ml
::Matrix,int> inertiaSensorSOUT;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
momentumSOUT
;
dg
::
SignalPtr
<
ml
::
Vector
,
int
>
momentumSIN
;
//dg::SignalTimeDependent<
dynamicgraph
::Matrix,int> inertiaSensorSOUT;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
momentumSOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
momentumSIN
;
/* --- OUTPUTS --- */
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
torsorCompensatedSOUT
;
dg
::
SignalTimeDependent
<
ml
::
Vector
,
int
>
torsorDeadZoneSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
torsorCompensatedSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
torsorDeadZoneSOUT
;
typedef
int
sotDummyType
;
dg
::
SignalTimeDependent
<
sotDummyType
,
int
>
calibrationTrigerSOUT
;
...
...
include/sot-dynamic/integrator-force-exact.h
View file @
290b0d23
...
...
@@ -26,16 +26,15 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include
<
jrl/mal/boost.h
h>
namespace
ml
=
maal
::
boost
;
#include
<
dynamic-graph/linear-algebra.
h>
/* SOT */
#include
<dynamic-graph/entity.h>
#include
<dynamic-graph/signal-ptr.h>
#include
<dynamic-graph/signal-time-dependent.h>
#include
<sot/core/matrix-homogeneous.hh>
#include
<sot/core/vector-roll-pitch-yaw.hh>
#include
<sot/core/matrix-rotation.hh>
#include
<sot/core/matrix-geometry.hh>
#include
<sot-dynamic/integrator-force.h>
/* STD */
...
...
@@ -80,7 +79,7 @@ class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
public:
/* --- FUNCTIONS --- */
ml
::
Vector
&
computeVelocityExact
(
ml
::
Vector
&
res
,
dynamicgraph
::
Vector
&
computeVelocityExact
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
/* public: /\* --- PARAMS --- *\/ */
...
...
include/sot-dynamic/integrator-force-rk4.h
View file @
290b0d23
...
...
@@ -26,16 +26,15 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include
<
jrl/mal/boost.h
h>
namespace
ml
=
maal
::
boost
;
#include
<
dynamic-graph/linear-algebra.
h>
/* SOT */
#include
<dynamic-graph/entity.h>
#include
<dynamic-graph/signal-ptr.h>
#include
<dynamic-graph/signal-time-dependent.h>
#include
<sot/core/matrix-homogeneous.hh>
#include
<sot/core/vector-roll-pitch-yaw.hh>
#include
<sot/core/matrix-rotation.hh>
#include
<sot/core/matrix-geometry.hh>
#include
<sot-dynamic/integrator-force.h>
/* STD */
...
...
@@ -80,7 +79,7 @@ class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4
public:
/* --- FUNCTIONS --- */
ml
::
Vector
&
computeDerivativeRK4
(
ml
::
Vector
&
res
,
dynamicgraph
::
Vector
&
computeDerivativeRK4
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
/* public: /\* --- PARAMS --- *\/ */
...
...
include/sot-dynamic/integrator-force.h
View file @
290b0d23
...
...
@@ -26,16 +26,15 @@
/* --------------------------------------------------------------------- */
/* Matrix */
#include
<
jrl/mal/boost.h
h>
namespace
ml
=
maal
::
boost
;
#include
<
dynamic-graph/linear-algebra.
h>
/* SOT */
#include
<dynamic-graph/entity.h>
#include
<dynamic-graph/signal-ptr.h>
#include
<dynamic-graph/signal-time-dependent.h>
#include
<sot/core/matrix-homogeneous.hh>
#include
<sot/core/vector-roll-pitch-yaw.hh>
#include
<sot/core/matrix-rotation.hh>
#include
<sot/core/matrix-geometry.hh>
/* STD */
#include
<string>
...