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
d5062fa6
Commit
d5062fa6
authored
Aug 13, 2018
by
Guilhem Saurel
Browse files
Remove shell
ref
https://github.com/stack-of-tasks/sot-core/issues/58
parent
4d0349ba
Changes
17
Hide whitespace changes
Inline
Side-by-side
include/sot-dynamic-pinocchio/angle-estimator.h
View file @
d5062fa6
...
...
@@ -114,9 +114,6 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
public:
/* --- PARAMS --- */
void
fromSensor
(
const
bool
&
fs
)
{
fromSensor_
=
fs
;
}
bool
fromSensor
()
const
{
return
fromSensor_
;
}
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
private:
bool
fromSensor_
;
...
...
include/sot-dynamic-pinocchio/dynamic-pinocchio.h
View file @
d5062fa6
...
...
@@ -67,10 +67,10 @@
#endif
namespace
dynamicgraph
{
namespace
dynamicgraph
{
namespace
sot
{
namespace
dg
=
dynamicgraph
;
namespace
command
{
class
SetFile
;
class
CreateOpPoint
;
...
...
@@ -78,9 +78,9 @@ namespace dynamicgraph {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup signals
\brief This class provides an inverse dynamic model of the robot.
More precisely it wraps the newton euler algorithm implemented
...
...
@@ -92,8 +92,8 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
friend
class
sot
::
command
::
SetFile
;
friend
class
sot
::
command
::
CreateOpPoint
;
// friend class sot::command::InitializeRobot;
public:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DYNAMIC_GRAPH_ENTITY_DECL
();
...
...
@@ -110,15 +110,15 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg
::
SignalTimeDependent
<
dg
::
Matrix
,
int
>&
createJacobianSignal
(
const
std
::
string
&
signame
,
const
std
::
string
&
);
void
destroyJacobianSignal
(
const
std
::
string
&
signame
);
dg
::
SignalTimeDependent
<
MatrixHomogeneous
,
int
>&
createPositionSignal
(
const
std
::
string
&
,
const
std
::
string
&
);
void
destroyPositionSignal
(
const
std
::
string
&
signame
);
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>&
createVelocitySignal
(
const
std
::
string
&
,
const
std
::
string
&
);
void
destroyVelocitySignal
(
const
std
::
string
&
signame
);
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>&
createAccelerationSignal
(
const
std
::
string
&
,
const
std
::
string
&
);
void
destroyAccelerationSignal
(
const
std
::
string
&
signame
);
...
...
@@ -137,7 +137,7 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg
::
SignalPtr
<
dg
::
Vector
,
int
>
freeFlyerVelocitySIN
;
dg
::
SignalPtr
<
dg
::
Vector
,
int
>
jointAccelerationSIN
;
dg
::
SignalPtr
<
dg
::
Vector
,
int
>
freeFlyerAccelerationSIN
;
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
pinocchioPosSINTERN
;
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
pinocchioVelSINTERN
;
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
pinocchioAccSINTERN
;
...
...
@@ -151,17 +151,17 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
int
&
computeForwardKinematics
(
int
&
dummy
,
const
int
&
time
);
int
&
computeCcrba
(
int
&
dummy
,
const
int
&
time
);
int
&
computeJacobians
(
int
&
dummy
,
const
int
&
time
);
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
zmpSOUT
;
dg
::
SignalTimeDependent
<
dg
::
Matrix
,
int
>
JcomSOUT
;
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
comSOUT
;
dg
::
SignalTimeDependent
<
dg
::
Matrix
,
int
>
inertiaSOUT
;
dg
::
SignalTimeDependent
<
dg
::
Matrix
,
int
>&
jacobiansSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
MatrixHomogeneous
,
int
>&
positionsSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>&
velocitiesSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>&
accelerationsSOUT
(
const
std
::
string
&
name
);
dg
::
SignalTimeDependent
<
double
,
int
>
footHeightSOUT
;
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
upperJlSOUT
;
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
lowerJlSOUT
;
...
...
@@ -184,14 +184,14 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
/* --- MODEL CREATION --- */
void
displayModel
()
const
void
displayModel
()
const
{
assert
(
m_model
);
std
::
cout
<<
(
*
m_model
)
<<
std
::
endl
;
};
void
setModel
(
se3
::
Model
*
);
void
setData
(
se3
::
Data
*
);
/* --- GETTERS --- */
/// \brief Get joint position lower limits
...
...
@@ -228,7 +228,7 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg
::
Matrix
&
computeGenericEndeffJacobian
(
const
bool
isFrame
,
const
int
jointId
,
dg
::
Matrix
&
res
,
const
int
&
time
);
MatrixHomogeneous
&
computeGenericPosition
(
const
bool
isFrame
,
MatrixHomogeneous
&
computeGenericPosition
(
const
bool
isFrame
,
const
int
jointId
,
MatrixHomogeneous
&
res
,
const
int
&
time
);
dg
::
Vector
&
computeGenericVelocity
(
const
int
jointId
,
dg
::
Vector
&
res
,
const
int
&
time
);
...
...
@@ -246,9 +246,6 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg
::
Vector
&
computeTorqueDrift
(
dg
::
Vector
&
res
,
const
int
&
time
);
public:
/* --- PARAMS --- */
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
void
cmd_createOpPointSignals
(
const
std
::
string
&
sig
,
const
std
::
string
&
j
);
void
cmd_createJacobianWorldSignal
(
const
std
::
string
&
sig
,
const
std
::
string
&
j
);
void
cmd_createJacobianEndEffectorSignal
(
const
std
::
string
&
sig
,
const
std
::
string
&
j
);
...
...
@@ -258,11 +255,11 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
private:
/// \brief map of joints in construction.
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
dg
::
Vector
&
getPinocchioPos
(
dg
::
Vector
&
q
,
const
int
&
time
);
dg
::
Vector
&
getPinocchioVel
(
dg
::
Vector
&
v
,
const
int
&
time
);
dg
::
Vector
&
getPinocchioAcc
(
dg
::
Vector
&
a
,
const
int
&
time
);
//\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
std
::
vector
<
int
>
sphericalJoints
;
...
...
include/sot-dynamic-pinocchio/force-compensation.h
View file @
d5062fa6
...
...
@@ -43,12 +43,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (force_compensation_EXPORTS)
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
# else
# else
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTFORCECOMPENSATION_EXPORT
#endif
...
...
@@ -70,12 +70,12 @@ namespace dynamicgraph { namespace sot {
public:
ForceCompensation
(
void
);
static
MatrixForce
&
computeHandXworld
(
static
MatrixForce
&
computeHandXworld
(
const
MatrixRotation
&
worldRhand
,
const
dynamicgraph
::
Vector
&
transSensorCom
,
MatrixForce
&
res
);
static
MatrixForce
&
computeHandVsensor
(
const
MatrixRotation
&
sensorRhand
,
MatrixForce
&
res
);
static
MatrixForce
&
computeSensorXhand
(
const
MatrixRotation
&
sensorRhand
,
...
...
@@ -106,7 +106,7 @@ namespace dynamicgraph { namespace sot {
static
dynamicgraph
::
Vector
&
computeDeadZone
(
const
dynamicgraph
::
Vector
&
torqueInput
,
const
dynamicgraph
::
Vector
&
deadZoneLimit
,
dynamicgraph
::
Vector
&
res
);
public:
// CALIBRATION
std
::
list
<
dynamicgraph
::
Vector
>
torsorList
;
...
...
@@ -115,15 +115,15 @@ namespace dynamicgraph { namespace sot {
void
clearCalibration
(
void
);
void
addCalibrationValue
(
const
dynamicgraph
::
Vector
&
torsor
,
const
MatrixRotation
&
worldRhand
);
dynamicgraph
::
Vector
calibrateTransSensorCom
(
const
dynamicgraph
::
Vector
&
gravity
,
const
MatrixRotation
&
handRsensor
);
dynamicgraph
::
Vector
calibrateGravity
(
const
MatrixRotation
&
handRsensor
,
bool
precompensationCalibration
=
false
,
const
MatrixRotation
&
hand0Rsensor
=
I3
);
};
/* --------------------------------------------------------------------- */
...
...
@@ -147,50 +147,43 @@ namespace dynamicgraph { namespace sot {
public:
/* --- SIGNAL --- */
/* --- INPUTS --- */
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
torsorSIN
;
dg
::
SignalPtr
<
MatrixRotation
,
int
>
worldRhandSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
torsorSIN
;
dg
::
SignalPtr
<
MatrixRotation
,
int
>
worldRhandSIN
;
/* --- CONSTANTS --- */
dg
::
SignalPtr
<
MatrixRotation
,
int
>
handRsensorSIN
;
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
<
dynamicgraph
::
Vector
,
int
>
velocitySIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
accelerationSIN
;
dg
::
SignalPtr
<
MatrixRotation
,
int
>
handRsensorSIN
;
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
<
dynamicgraph
::
Vector
,
int
>
velocitySIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
accelerationSIN
;
/* --- INTERMEDIATE OUTPUTS --- */
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
handXworldSOUT
;
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
handVsensorSOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
torsorDeadZoneSIN
;
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
handXworldSOUT
;
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
handVsensorSOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
torsorDeadZoneSIN
;
dg
::
SignalTimeDependent
<
MatrixForce
,
int
>
sensorXhandSOUT
;
//dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
momentumSOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
momentumSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
momentumSOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
momentumSIN
;
/* --- OUTPUTS --- */
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
torsorCompensatedSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
torsorCompensatedSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
torsorDeadZoneSOUT
;
typedef
int
sotDummyType
;
dg
::
SignalTimeDependent
<
sotDummyType
,
int
>
calibrationTrigerSOUT
;
dg
::
SignalTimeDependent
<
sotDummyType
,
int
>
calibrationTrigerSOUT
;
public:
/* --- COMMANDLINE --- */
sotDummyType
&
calibrationTriger
(
sotDummyType
&
dummy
,
int
time
);
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
};
...
...
include/sot-dynamic-pinocchio/integrator-force-exact.h
View file @
d5062fa6
...
...
@@ -44,12 +44,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (integrator_force_exact_EXPORTS)
# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllexport)
# else
# else
# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTINTEGRATORFORCEEXACT_EXPORT
#endif
...
...
@@ -81,12 +81,6 @@ class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
public:
/* --- FUNCTIONS --- */
dynamicgraph
::
Vector
&
computeVelocityExact
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
/* public: /\* --- PARAMS --- *\/ */
/* virtual void commandLine( const std::string& cmdLine, */
/* std::istringstream& cmdArgs, */
/* std::ostream& os ); */
};
...
...
include/sot-dynamic-pinocchio/integrator-force-rk4.h
View file @
d5062fa6
...
...
@@ -44,12 +44,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (integrator_force_rk4_EXPORTS)
# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllexport)
# else
# else
# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTINTEGRATORFORCERK4_EXPORT
#endif
...
...
@@ -81,12 +81,6 @@ class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4
public:
/* --- FUNCTIONS --- */
dynamicgraph
::
Vector
&
computeDerivativeRK4
(
dynamicgraph
::
Vector
&
res
,
const
int
&
time
);
/* public: /\* --- PARAMS --- *\/ */
/* virtual void commandLine( const std::string& cmdLine, */
/* std::istringstream& cmdArgs, */
/* std::ostream& os ); */
};
...
...
include/sot-dynamic-pinocchio/integrator-force.h
View file @
d5062fa6
...
...
@@ -43,12 +43,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (integrator_force_EXPORTS)
# define SOTINTEGRATORFORCE_EXPORT __declspec(dllexport)
# else
# else
# define SOTINTEGRATORFORCE_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTINTEGRATORFORCE_EXPORT
#endif
...
...
@@ -80,18 +80,18 @@ class SOTINTEGRATORFORCE_EXPORT IntegratorForce
public:
/* --- SIGNAL --- */
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
forceSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
massInverseSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
frictionSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
forceSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
massInverseSIN
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
frictionSIN
;
/* Memory of the previous iteration. The sig is fed by the previous
* computations. */
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
velocityPrecSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
velocityDerivativeSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
velocitySOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
velocityPrecSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
velocityDerivativeSOUT
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
velocitySOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
massSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>
massInverseSOUT
;
dg
::
SignalPtr
<
dynamicgraph
::
Matrix
,
int
>
massSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Matrix
,
int
>
massInverseSOUT
;
public:
/* --- FUNCTIONS --- */
dynamicgraph
::
Vector
&
computeDerivative
(
dynamicgraph
::
Vector
&
res
,
...
...
@@ -102,13 +102,6 @@ class SOTINTEGRATORFORCE_EXPORT IntegratorForce
dynamicgraph
::
Matrix
&
computeMassInverse
(
dynamicgraph
::
Matrix
&
res
,
const
int
&
time
);
public:
/* --- PARAMS --- */
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
};
...
...
include/sot-dynamic-pinocchio/waist-attitude-from-sensor.h
View file @
d5062fa6
...
...
@@ -42,12 +42,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (waist_attitude_from_sensor_EXPORTS)
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllexport)
# else
# else
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT
#endif
...
...
@@ -81,13 +81,6 @@ class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistAttitudeFromSensor
dg
::
SignalPtr
<
MatrixHomogeneous
,
int
>
positionSensorSIN
;
dg
::
SignalTimeDependent
<
VectorRollPitchYaw
,
int
>
attitudeWaistSOUT
;
public:
/* --- PARAMS --- */
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
};
...
...
@@ -120,13 +113,6 @@ class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistPoseFromSensorAndContact
dg
::
SignalPtr
<
MatrixHomogeneous
,
int
>
positionContactSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
positionWaistSOUT
;
public:
/* --- PARAMS --- */
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
};
...
...
include/sot-dynamic-pinocchio/zmpreffromcom.h
View file @
d5062fa6
...
...
@@ -42,12 +42,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (zmpreffromcom_EXPORTS)
# define SOTZMPREFFROMCOM_EXPORT __declspec(dllexport)
# else
# else
# define SOTZMPREFFROMCOM_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTZMPREFFROMCOM_EXPORT
#endif
...
...
@@ -69,7 +69,7 @@ class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom
double
dt
;
const
static
double
DT_DEFAULT
;
// = 5e-3; // 5ms
double
footHeight
;
const
static
double
FOOT_HEIGHT_DEFAULT
;
// = .105;
const
static
double
FOOT_HEIGHT_DEFAULT
;
// = .105;
public:
/* --- CONSTRUCTION --- */
...
...
@@ -86,13 +86,6 @@ class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom
dg
::
SignalPtr
<
dynamicgraph
::
Vector
,
int
>
dcomSIN
;
dg
::
SignalTimeDependent
<
dynamicgraph
::
Vector
,
int
>
zmprefSOUT
;
public:
/* --- PARAMS --- */
virtual
void
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
);
};
...
...
src/angle-estimator.cpp
View file @
d5062fa6
...
...
@@ -315,7 +315,7 @@ computeWaistWorldPosition( MatrixHomogeneous& res,
if
(
fromSensor_
)
{
const
MatrixRotation
&
Rflex
=
flexibilitySOUT
(
time
);
// footRleg
MatrixHomogeneous
footMleg
;
MatrixHomogeneous
footMleg
;
footMleg
.
linear
()
=
Rflex
;
footMleg
.
translation
().
setZero
();
tmpRes
=
footMleg
*
legMwaist
;
...
...
@@ -384,31 +384,3 @@ compute_qdotSOUT( dynamicgraph::Vector& res,
return
res
;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
void
AngleEstimator
::
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
)
{
sotDEBUG
(
25
)
<<
"Cmd "
<<
cmdLine
<<
std
::
endl
;
if
(
cmdLine
==
"help"
)
{
Entity
::
commandLine
(
cmdLine
,
cmdArgs
,
os
);
}
else
if
(
cmdLine
==
"fromSensor"
)
{
std
::
string
val
;
cmdArgs
>>
val
;
if
(
(
"true"
==
val
)
||
(
"false"
==
val
)
)
{
fromSensor_
=
(
val
==
"true"
);
}
else
{
os
<<
"fromSensor = "
<<
(
fromSensor_
?
"true"
:
"false"
)
<<
std
::
endl
;
}
}
else
{
Entity
::
commandLine
(
cmdLine
,
cmdArgs
,
os
);
}
}
src/force-compensation.cpp
View file @
d5062fa6
...
...
@@ -331,7 +331,7 @@ computeHandXworld( const MatrixRotation & worldRhand,
sotDEBUG
(
25
)
<<
"wRrh = "
<<
worldRhand
<<
std
::
endl
;
sotDEBUG
(
25
)
<<
"SC = "
<<
transSensorCom
<<
std
::
endl
;
MatrixHomogeneous
scRw
;
MatrixHomogeneous
scRw
;
scRw
.
linear
()
=
worldRhand
.
transpose
();
scRw
.
translation
()
=
transSensorCom
;
sotDEBUG
(
25
)
<<
"scMw = "
<<
scRw
<<
std
::
endl
;
...
...
@@ -344,7 +344,7 @@ computeHandXworld( const MatrixRotation & worldRhand,
_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
).
setZero
();
res
.
block
<
3
,
3
>
(
3
,
0
)
=
sk
;
...
...
@@ -395,7 +395,7 @@ computeSensorXhand( const MatrixRotation & /*handRsensor*/,
Tx
<<
0
,
-
transJointSensor
(
2
),
transJointSensor
(
1
),
transJointSensor
(
2
),
0
,
-
transJointSensor
(
0
),
-
transJointSensor
(
1
),
transJointSensor
(
0
),
0
;
res
.
block
<
3
,
3
>
(
0
,
0
).
setIdentity
();
res
.
block
<
3
,
3
>
(
0
,
3
).
setZero
();
res
.
block
<
3
,
3
>
(
3
,
0
)
=
Tx
;
...
...
@@ -444,7 +444,7 @@ computeTorsorCompensated( const dynamicgraph::Vector& torqueInput,
sotDEBUG
(
25
)
<<
"t_nc = "
<<
torqueInput
;
dynamicgraph
::
Vector
torquePrecompensated
(
6
);
//if( usingPrecompensation )
torquePrecompensated
=
torqueInput
+
torquePrecompensation
;
torquePrecompensated
=
torqueInput
+
torquePrecompensation
;
//else { torquePrecompensated = torqueInput; }
sotDEBUG
(
25
)
<<
"t_pre = "
<<
torquePrecompensated
;
...
...
@@ -496,7 +496,7 @@ crossProduct_V_F( const dynamicgraph::Vector& velocity,
}
return
res
;
}
dynamicgraph
::
Vector
&
ForceCompensation
::
computeMomentum
(
const
dynamicgraph
::
Vector
&
velocity
,
...
...
@@ -553,115 +553,3 @@ calibrationTriger( ForceCompensationPlugin::sotDummyType& dummy,int /*time*/ )
// sotDEBUGOUT(45);
return
dummy
=
1
;
}
/* --- COMMANDLINE ---------------------------------------------------------- */
/* --- COMMANDLINE ---------------------------------------------------------- */
/* --- COMMANDLINE ---------------------------------------------------------- */
void
ForceCompensationPlugin
::
commandLine
(
const
std
::
string
&
cmdLine
,
std
::
istringstream
&
cmdArgs
,
std
::
ostream
&
os
)
{
if
(
"help"
==
cmdLine
)
{
os
<<
"ForceCompensation: "
<<
" - clearCalibration"
<<
std
::
endl
<<
" - {start|stop}Calibration [wait <time_sec>]"
<<
std
::
endl
<<
" - calibrateGravity
\t
[only {x|y|z}]"
<<
std
::
endl
<<
" - calibratePosition"
<<
std
::
endl
<<
" - precomp [{true|false}]: get/set the "
<<
"precompensation due to sensor calib."
<<
std
::
endl
;
}
// else if( "clearCalibration" == cmdLine )
// {
// clearCalibration();
// }
// else if( "startCalibration" == cmdLine )
// {
// calibrationStarted = true;
// cmdArgs >> std::ws;
// if( cmdArgs.good() )
// {
// std::string cmdWait; cmdArgs>>cmdWait>>std::ws;
// if( (cmdWait == "wait")&&(cmdArgs.good()) )
// {
// double timeSec; cmdArgs >> timeSec;
// unsigned int timeMSec= (unsigned int)(round(timeSec*1000*1000));
// sotDEBUG(15) << "Calibration: wait for " << timeMSec << "us."<<std::endl;
// usleep( timeMSec );
// calibrationStarted = false;
// }
// }
// }
// else if( "stopCalibration" == cmdLine )
// {