diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index 8810150db864e1aa688ffc0aebec4a379fe044c9..c9ca73a7c363526675ea504a76d6f9e7ac8d4b79 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -65,6 +65,11 @@ namespace djj = dynamicsJRLJapan;
namespace sot {
namespace dg = dynamicgraph;
+ namespace command {
+ class SetFiles;
+ class Parse;
+ class CreateOpPoint;
+ }
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -79,6 +84,9 @@ namespace dg = dynamicgraph;
class SOTDYNAMIC_EXPORT Dynamic
:public dg::Entity
{
+ friend class sot::command::SetFiles;
+ friend class sot::command::Parse;
+ friend class sot::command::CreateOpPoint;
public:
static const std::string CLASS_NAME;
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a1ffc61217fb924b52edb0705c1b41132ce20b42..f83cc04bbfac169c66df61eea9f64de0406837c5 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,4 +1,5 @@
-# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
+# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST,
+# Florent Lamiraux (CNRS/LAAS)
#
# This file is part of sot-dynamic.
# sot-dynamic is free software: you can redistribute it and/or
@@ -13,6 +14,8 @@
# received a copy of the GNU Lesser General Public License along with
# sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
+INCLUDE(../cmake/python.cmake)
+
IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
ADD_DEFINITIONS(-DDEBUG=2)
ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
@@ -43,8 +46,27 @@ FOREACH(lib ${libs})
PKG_CONFIG_USE_DEPENDENCY(${lib} jrl-mal)
INSTALL(TARGETS ${lib} DESTINATION lib/plugin)
+ # build python submodule
+ STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
+ ADD_DEPENDENCIES(${lib} MKDIR_${PYTHON_LIBRARY_NAME})
+ ADD_CUSTOM_TARGET(MKDIR_${PYTHON_LIBRARY_NAME}
+ mkdir -p dynamic_graph/sot/dynamics/${PYTHON_LIBRARY_NAME}
+ )
+ DYNAMIC_GRAPH_PYTHON_MODULE("sot/dynamics/${PYTHON_LIBRARY_NAME}"
+ ${lib}
+ sot/dynamics/${PYTHON_LIBRARY_NAME}/wrap
+ )
ENDFOREACH(lib)
+EXEC_PROGRAM(${PYTHON_EXECUTABLE} ARGS "-c \"from distutils import sysconfig; print sysconfig.get_python_lib(0,0,prefix='')\""
+ OUTPUT_VARIABLE PYTHON_SITELIB
+ )
+
+# Install empty __init__.py files in intermediate directories.
+INSTALL(FILES ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/__init__.py
+ DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dynamics
+ )
+
PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2 hrp2-dynamics)
IF(${HRP2_10_OPTIMIZED_FOUND})
diff --git a/src/dynamic-command.h b/src/dynamic-command.h
new file mode 100644
index 0000000000000000000000000000000000000000..f640a70b86ebbb949666f87abcf2b5a77d7ff949
--- /dev/null
+++ b/src/dynamic-command.h
@@ -0,0 +1,188 @@
+/*
+ * Copyright 2010,
+ * Florent Lamiraux
+ *
+ * CNRS/AIST
+ *
+ * This file is part of sot-core.
+ * sot-core is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-core is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-core. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef DYNAMIC_COMMAND_H
+ #define DYNAMIC_COMMAND_H
+
+ #include <boost/assign/list_of.hpp>
+
+ #include <dynamic-graph/command.h>
+ #include <dynamic-graph/command-setter.h>
+ #include <dynamic-graph/command-getter.h>
+
+namespace sot {
+ namespace command {
+ using ::dynamicgraph::command::Command;
+ using ::dynamicgraph::command::Value;
+
+ // Command SetFiles
+ class SetFiles : public Command
+ {
+ public:
+ virtual ~SetFiles() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ SetFiles(Dynamic& entity, const std::string& docstring) :
+ Command(entity, boost::assign::list_of(Value::STRING)
+ (Value::STRING)(Value::STRING)(Value::STRING), docstring)
+ {
+ }
+ virtual Value doExecute()
+ {
+ Dynamic& robot = static_cast<Dynamic&>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string vrmlDirectory = values[0].value();
+ std::string vrmlMainFile = values[1].value();
+ std::string xmlSpecificityFiles = values[2].value();
+ std::string xmlRankFile = values[3].value();
+ robot.setVrmlDirectory(vrmlDirectory);
+ robot.setVrmlMainFile(vrmlMainFile);
+ robot.setXmlSpecificityFile(xmlSpecificityFiles);
+ robot.setXmlRankFile(xmlRankFile);
+ // return void
+ return Value();
+ }
+ }; // class SetFiles
+
+ // Command Parse
+ class Parse : public Command
+ {
+ public:
+ virtual ~Parse() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ Parse(Dynamic& entity, const std::string& docstring) :
+ Command(entity, std::vector<Value::Type>(), docstring)
+ {
+ }
+ virtual Value doExecute()
+ {
+ Dynamic& robot = static_cast<Dynamic&>(owner());
+ if(! robot.init ) robot.parseConfigFiles();
+ else std::cout << " !! Already parsed." << std::endl;
+ // return void
+ return Value();
+ }
+ }; // class Parse
+
+ // Command CreateOpPoint
+ class CreateOpPoint : public Command
+ {
+ public:
+ virtual ~CreateOpPoint() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ CreateOpPoint(Dynamic& entity, const std::string& docstring) :
+ Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
+ docstring)
+ {
+ }
+ virtual Value doExecute()
+ {
+ Dynamic& robot = static_cast<Dynamic&>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string opPointName = values[0].value();
+ std::string jointName = values[1].value();
+ std::cout << "opPointName = " << opPointName << std::endl;
+ std::cout << "jointName = " << jointName << std::endl;
+ CjrlJoint* joint = NULL;
+ if (jointName == "gaze") {
+ joint = robot.m_HDR->gazeJoint();
+ } else if (jointName == "left-ankle") {
+ joint = robot.m_HDR->leftAnkle();
+ } else if (jointName == "right-ankle") {
+ joint = robot.m_HDR->rightAnkle();
+ } else if (jointName == "left-wrist") {
+ joint = robot.m_HDR->leftWrist();
+ } else if (jointName == "right-wrist") {
+ joint = robot.m_HDR->rightWrist();
+ } else if (jointName == "waist") {
+ joint = robot.m_HDR->waist();
+ } else if (jointName == "chest") {
+ joint = robot.m_HDR->chest();
+ } else {
+ throw ExceptionDynamic(ExceptionDynamic::GENERIC,
+ jointName + " is not a valid name."
+ " Valid names are \n"
+ "gaze, left-ankle, right ankle, left-wrist,"
+ " right-wrist, waist, chest.");
+ }
+ robot.createEndeffJacobianSignal(std::string("J")+opPointName, joint);
+ robot.createPositionSignal(opPointName, joint);
+ return Value();
+ }
+ }; // class CreateOpPoint
+
+ // Command SetProperty
+ class SetProperty : public Command
+ {
+ public:
+ virtual ~SetProperty() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ SetProperty(Dynamic& entity, const std::string& docstring) :
+ Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
+ docstring)
+ {
+ }
+ virtual Value doExecute()
+ {
+ Dynamic& robot = static_cast<Dynamic&>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string property = values[0].value();
+ std::string value = values[1].value();
+
+ robot.m_HDR->setProperty(property, value);
+ return Value();
+ }
+ }; // class SetProperty
+
+ // Command GetProperty
+ class GetProperty : public Command
+ {
+ public:
+ virtual ~GetProperty() {}
+ /// Create command and store it in Entity
+ /// \param entity instance of Entity owning this command
+ /// \param docstring documentation of the command
+ GetProperty(Dynamic& entity, const std::string& docstring) :
+ Command(entity, boost::assign::list_of(Value::STRING),
+ docstring)
+ {
+ }
+ virtual Value doExecute()
+ {
+ Dynamic& robot = static_cast<Dynamic&>(owner());
+ std::vector<Value> values = getParameterValues();
+ std::string property = values[0].value();
+ std::string value;
+
+ robot.m_HDR->getProperty(property, value);
+ return Value(value);
+ }
+ }; // class GetProperty
+
+ } // namespace command
+} //namespace sot
+
+#endif //DYNAMIC_COMMAND_H
diff --git a/src/dynamic.cpp b/src/dynamic.cpp
index d095a985e86461a34eabda47a9e6ef3163375ebb..d1e73eef150c79c864019f4fe55e7584b3796328 100644
--- a/src/dynamic.cpp
+++ b/src/dynamic.cpp
@@ -28,6 +28,8 @@
#include <dynamic-graph/factory.h>
+#include "../src/dynamic-command.h"
+
using namespace sot;
using namespace dynamicgraph;
@@ -113,6 +115,56 @@ Dynamic( const std::string & name, bool build )
<<inertiaRealSOUT << inertiaRotorSOUT << gearRatioSOUT );
signalRegistration( MomentaSOUT << AngularMomentumSOUT );
+ //
+ // Commands
+ //
+ std::string docstring;
+ // setFiles
+ docstring =
+ "\n"
+ " Define files to parse in order to build the robot.\n"
+ "\n"
+ " Takes as input 4 string:\n"
+ " - Directory containing main wrl file,\n"
+ " - name of wrl file,\n"
+ " - xml file containing humanoid specificities,\n"
+ " - xml file defining order of joints in configuration vector.\n"
+ "\n";
+ addCommand("setFiles",
+ new command::SetFiles(*this, docstring));
+ // parse
+ docstring =
+ "\n"
+ " Parse files to build an instance ot robot.\n"
+ "\n"
+ " Takes no argument.\n"
+ " Files are defined by command setFiles \n"
+ "\n";
+ addCommand("parse",
+ new command::Parse(*this, docstring));
+
+ // CreateOpPoint
+ docstring = " \n"
+ " Create an operational point attached to a robot joint local frame.\n"
+ " \n"
+ " Takes 2 arguments: \n"
+ " - a string: name of the operational point,\n"
+ " - a string: name the joint, among (gaze, left-ankle, right ankle\n"
+ " , left-wrist, right-wrist, waist, chest).\n"
+ "\n";
+ addCommand("createOpPoint",
+ new command::CreateOpPoint(*this, docstring));
+
+ // SetProperty
+ docstring = " \n"
+ " Set a property.\n"
+ " \n"
+ " Takes 2 arguments:\n"
+ " - a string: name of the property,\n"
+ " - a string: value of the property.\n"
+ " \n";
+ addCommand("setProperty",
+ new command::SetProperty(*this, docstring));
sotDEBUGOUT(5);
}
diff --git a/src/dynamic_graph/sot/dynamics/__init__.py b/src/dynamic_graph/sot/dynamics/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391