Commit ab9c3de1 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Add a command to get hand parameters.

parent c03bf5b2
......@@ -491,6 +491,41 @@ namespace dynamicgraph { namespace sot {
return Value();
}
}; // class Write
class GetHandParameter : public Command
{
public:
virtual ~GetHandParameter () {}
GetHandParameter (Dynamic& entity, const std::string& docstring) :
Command (entity, boost::assign::list_of(Value::BOOL), docstring)
{
}
virtual Value doExecute ()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
bool right = values [0].value ();
ml::Matrix handParameter (4,4);
handParameter.setIdentity ();
CjrlHand* hand;
if (right) hand = robot.m_HDR->rightHand ();
else hand = robot.m_HDR->leftHand ();
vector3d axis;
hand->getThumbAxis (axis);
for (unsigned int i=0; i<3; i++)
handParameter (i,0) = axis (i);
hand->getForeFingerAxis (axis);
for (unsigned int i=0; i<3; i++)
handParameter (i,1) = axis (i);
hand->getPalmNormal (axis);
for (unsigned int i=0; i<3; i++)
handParameter (i,2) = axis (i);
hand->getCenter (axis);
for (unsigned int i=0; i<3; i++)
handParameter (i,3) = axis (i);
return Value (handParameter);
}
}; // class GetHandParameter
} // namespace command
} /* namespace sot */} /* namespace dynamicgraph */
......
......@@ -412,6 +412,21 @@ Dynamic( const std::string & name, bool build )
new dynamicgraph::command::Getter<Dynamic, ml::Vector>
(*this, &Dynamic::getAnklePositionInFootFrame, docstring));
docstring = " \n"
" Get geometric parameters of hand.\n"
" \n"
" Input\n"
" - a boolean: whether right foot or not,\n"
" Return\n"
" - a matrix 4 by 4 the columns of which respectively represent\n"
" - the thumb axis,\n"
" - the forefinger axis,\n"
" - the palm normal,\n"
" - the hand center.\n"
" Note that the last line is (0 0 0 1).\n"
" \n";
addCommand ("getHandParameter",
new command::GetHandParameter (*this, docstring));
sotDEBUGOUT(5);
}
......
Supports Markdown
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