diff --git a/src/dynamic-command.h b/src/dynamic-command.h index 138def4265ca45f4acc0de9c02f1c9e6347d0152..1b39d28c4326f72e539edbbe93b17befc4287249 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 dd1ae977c1d50aa6fd6a91f2b9d3e415c7066125..b482da7464c4e35c1eabf54b1dcfc418ea1866d1 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); }