Add a command to get hand parameters.

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 */
 

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);
 }