From ab9c3de12e9be516f42bd3dad8f0650bb231e7c0 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Thu, 15 Mar 2012 14:15:33 +0100
Subject: [PATCH] Add a command to get hand parameters.
---
src/dynamic-command.h | 35 +++++++++++++++++++++++++++++++++++
src/dynamic.cpp | 15 +++++++++++++++
2 files changed, 50 insertions(+)
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
index 138def4..1b39d28 100644
--- a/src/dynamic-command.h
+++ b/src/dynamic-command.h
@@ -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 */
diff --git a/src/dynamic.cpp b/src/dynamic.cpp
index dd1ae97..b482da7 100644
--- a/src/dynamic.cpp
+++ b/src/dynamic.cpp
@@ -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);
}
--
GitLab