Commit 4a4e1fe1 authored by florent's avatar florent
Browse files

Implement 3 commands and bind them in python.

    * include/sot-dynamic/dynamic.h,
    * src/dynamic-command.h,
    * src/dynamic.cpp: commands SetFiles, Parse, CreateOpPoint.
    * src/CMakeLists.txt,
    * src/dynamic_graph/sot/dynamics/__init__.py: new, Implement and install
      python submodule.
parent 5a60bb63
...@@ -65,6 +65,11 @@ namespace djj = dynamicsJRLJapan; ...@@ -65,6 +65,11 @@ namespace djj = dynamicsJRLJapan;
namespace sot { namespace sot {
namespace dg = dynamicgraph; namespace dg = dynamicgraph;
namespace command {
class SetFiles;
class Parse;
class CreateOpPoint;
}
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */
...@@ -79,6 +84,9 @@ namespace dg = dynamicgraph; ...@@ -79,6 +84,9 @@ namespace dg = dynamicgraph;
class SOTDYNAMIC_EXPORT Dynamic class SOTDYNAMIC_EXPORT Dynamic
:public dg::Entity :public dg::Entity
{ {
friend class sot::command::SetFiles;
friend class sot::command::Parse;
friend class sot::command::CreateOpPoint;
public: public:
static const std::string CLASS_NAME; static const std::string CLASS_NAME;
......
# 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. # This file is part of sot-dynamic.
# sot-dynamic is free software: you can redistribute it and/or # sot-dynamic is free software: you can redistribute it and/or
...@@ -13,6 +14,8 @@ ...@@ -13,6 +14,8 @@
# received a copy of the GNU Lesser General Public License along with # received a copy of the GNU Lesser General Public License along with
# sot-dynamic. If not, see <http://www.gnu.org/licenses/>. # sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
INCLUDE(../cmake/python.cmake)
IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG") IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
ADD_DEFINITIONS(-DDEBUG=2) ADD_DEFINITIONS(-DDEBUG=2)
ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG") ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
...@@ -43,8 +46,27 @@ FOREACH(lib ${libs}) ...@@ -43,8 +46,27 @@ FOREACH(lib ${libs})
PKG_CONFIG_USE_DEPENDENCY(${lib} jrl-mal) PKG_CONFIG_USE_DEPENDENCY(${lib} jrl-mal)
INSTALL(TARGETS ${lib} DESTINATION lib/plugin) 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) 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) PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2 hrp2-dynamics)
IF(${HRP2_10_OPTIMIZED_FOUND}) IF(${HRP2_10_OPTIMIZED_FOUND})
......
/*
* 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
...@@ -28,6 +28,8 @@ ...@@ -28,6 +28,8 @@
#include <dynamic-graph/factory.h> #include <dynamic-graph/factory.h>
#include "../src/dynamic-command.h"
using namespace sot; using namespace sot;
using namespace dynamicgraph; using namespace dynamicgraph;
...@@ -113,6 +115,56 @@ Dynamic( const std::string & name, bool build ) ...@@ -113,6 +115,56 @@ Dynamic( const std::string & name, bool build )
<<inertiaRealSOUT << inertiaRotorSOUT << gearRatioSOUT ); <<inertiaRealSOUT << inertiaRotorSOUT << gearRatioSOUT );
signalRegistration( MomentaSOUT << AngularMomentumSOUT ); 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); sotDEBUGOUT(5);
} }
......
Markdown is supported
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