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
172a9835
Commit
172a9835
authored
Jan 18, 2011
by
Florent Lamiraux
Browse files
Add commands to Dynamic entity to get foot parameters.
parent
424a0591
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/sot-dynamic/dynamic.h
View file @
172a9835
...
...
@@ -330,6 +330,22 @@ class SOTDYNAMIC_EXPORT Dynamic
/// \param inGazeDirection direction of the gase in gaze joint local frame.
void
setGazeParameters
(
const
ml
::
Vector
&
inGazeOrigin
,
const
ml
::
Vector
&
inGazeDirection
);
/// \brief Get length of left foot sole.
///
/// The robot is assumed to be symmetric.
double
getSoleLength
()
const
;
/// \brief Get width of left foot sole.
///
/// The robot is assumed to be symmetric.
double
getSoleWidth
()
const
;
/// \brief Get left ankle position in foot frame
///
/// The robot is assumed to be symmetric.
ml
::
Vector
getAnklePositionInFootFrame
()
const
;
/// @}
///
private:
...
...
src/dynamic.cpp
View file @
172a9835
...
...
@@ -354,6 +354,26 @@ Dynamic( const std::string & name, bool build )
addCommand
(
"write"
,
new
command
::
Write
(
*
this
,
docstring
));
docstring
=
"
\n
"
" Get left foot sole length.
\n
"
"
\n
"
;
addCommand
(
"getSoleLength"
,
new
dynamicgraph
::
command
::
Getter
<
Dynamic
,
double
>
(
*
this
,
&
Dynamic
::
getSoleLength
,
docstring
));
docstring
=
"
\n
"
" Get left foot sole width.
\n
"
"
\n
"
;
addCommand
(
"getSoleWidth"
,
new
dynamicgraph
::
command
::
Getter
<
Dynamic
,
double
>
(
*
this
,
&
Dynamic
::
getSoleWidth
,
docstring
));
docstring
=
"
\n
"
" Get ankle position in left foot frame.
\n
"
"
\n
"
;
addCommand
(
"getAnklePositionInFootFrame"
,
new
dynamicgraph
::
command
::
Getter
<
Dynamic
,
ml
::
Vector
>
(
*
this
,
&
Dynamic
::
getAnklePositionInFootFrame
,
docstring
));
sotDEBUGOUT
(
5
);
}
...
...
@@ -1603,6 +1623,58 @@ void Dynamic::setFootParameters(bool inRight, const double& inSoleLength,
foot
->
setAnklePositionInLocalFrame
(
maalToVector3d
(
inAnklePosition
));
}
double
Dynamic
::
getSoleLength
()
const
{
if
(
!
m_HDR
)
{
SOT_THROW
ExceptionDynamic
(
ExceptionDynamic
::
DYNAMIC_JRL
,
"you must create a robot first."
);
}
CjrlFoot
*
foot
=
m_HDR
->
leftFoot
();
if
(
!
foot
)
{
SOT_THROW
ExceptionDynamic
(
ExceptionDynamic
::
DYNAMIC_JRL
,
"left foot has not been defined yet"
);
}
double
length
,
width
;
foot
->
getSoleSize
(
length
,
width
);
return
length
;
}
double
Dynamic
::
getSoleWidth
()
const
{
if
(
!
m_HDR
)
{
SOT_THROW
ExceptionDynamic
(
ExceptionDynamic
::
DYNAMIC_JRL
,
"you must create a robot first."
);
}
CjrlFoot
*
foot
=
m_HDR
->
leftFoot
();
if
(
!
foot
)
{
SOT_THROW
ExceptionDynamic
(
ExceptionDynamic
::
DYNAMIC_JRL
,
"left foot has not been defined yet"
);
}
double
length
,
width
;
foot
->
getSoleSize
(
length
,
width
);
return
width
;
}
ml
::
Vector
Dynamic
::
getAnklePositionInFootFrame
()
const
{
if
(
!
m_HDR
)
{
SOT_THROW
ExceptionDynamic
(
ExceptionDynamic
::
DYNAMIC_JRL
,
"you must create a robot first."
);
}
CjrlFoot
*
foot
=
m_HDR
->
leftFoot
();
if
(
!
foot
)
{
SOT_THROW
ExceptionDynamic
(
ExceptionDynamic
::
DYNAMIC_JRL
,
"left foot has not been defined yet"
);
}
vector3d
anklePosition
;
foot
->
getAnklePositionInLocalFrame
(
anklePosition
);
ml
::
Vector
res
(
3
);
res
(
0
)
=
anklePosition
[
0
];
res
(
1
)
=
anklePosition
[
1
];
res
(
2
)
=
anklePosition
[
2
];
return
res
;
}
void
Dynamic
::
setGazeParameters
(
const
ml
::
Vector
&
inGazeOrigin
,
const
ml
::
Vector
&
inGazeDirection
)
{
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment