Commit 172a9835 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add commands to Dynamic entity to get foot parameters.

parent 424a0591
......@@ -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:
......
......@@ -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)
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment