Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • ostasse/dynamic-graph-python
  • gsaurel/dynamic-graph-python
  • stack-of-tasks/dynamic-graph-python
3 results
Show changes
Showing
with 1356 additions and 1008 deletions
// Copyright 2019, Olivier Stasse, LAAS-CNRS.
//
// See LICENSE
#include <iostream>
#define ENABLE_RT_LOG
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/real-time-logger.h>
#include <boost/shared_ptr.hpp>
#include <map>
#include <vector>
#include "dynamic-graph/python/dynamic-graph-py.hh"
typedef boost::shared_ptr<std::ofstream> ofstreamShrPtr;
namespace dynamicgraph {
namespace python {
namespace debug {
std::map<std::string, ofstreamShrPtr> mapOfFiles_;
void addLoggerFileOutputStream(const char* filename) {
std::ofstream* aofs = new std::ofstream;
ofstreamShrPtr ofs_shrptr = boost::shared_ptr<std::ofstream>(aofs);
aofs->open(filename, std::ofstream::out);
dynamicgraph::RealTimeLogger::instance();
dgADD_OSTREAM_TO_RTLOG(*aofs);
dgRTLOG() << "Added " << filename << " as an output stream \n";
mapOfFiles_[filename] = ofs_shrptr;
}
void closeLoggerFileOutputStream() {
for (const auto& el : mapOfFiles_) el.second->close();
}
void addLoggerCoutOutputStream() { dgADD_OSTREAM_TO_RTLOG(std::cout); }
void realTimeLoggerDestroy() { RealTimeLogger::destroy(); }
void realTimeLoggerSpinOnce() { RealTimeLogger::instance().spinOnce(); }
void realTimeLoggerInstance() { RealTimeLogger::instance(); }
} // namespace debug
} // namespace python
} // namespace dynamicgraph
// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.
#include "dynamic-graph/python/dynamic-graph-py.hh"
#include <dynamic-graph/command.h>
#include <dynamic-graph/debug.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/exception-factory.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/tracer.h>
#include <Eigen/Geometry>
#include <boost/python.hpp>
#include <boost/python/raw_function.hpp>
#include <boost/python/suite/indexing/map_indexing_suite.hpp>
#include <eigenpy/eigenpy.hpp>
#include <eigenpy/geometry.hpp>
#include <iostream>
#include <sstream>
#include "dynamic-graph/python/convert-dg-to-py.hh"
#include "dynamic-graph/python/module.hh"
#include "dynamic-graph/python/signal-wrapper.hh"
namespace dynamicgraph {
namespace python {
/**
\brief plug a signal into another one.
*/
void plug(SignalBase<int>* signalOut, SignalBase<int>* signalIn) {
signalIn->plug(signalOut);
}
void enableTrace(bool enable, const char* filename) {
if (enable)
DebugTrace::openFile(filename);
else
DebugTrace::closeFile(filename);
}
} // namespace python
} // namespace dynamicgraph
namespace bp = boost::python;
namespace dg = dynamicgraph;
typedef bp::return_value_policy<bp::manage_new_object> manage_new_object;
typedef bp::return_value_policy<bp::reference_existing_object>
reference_existing_object;
typedef dg::PoolStorage::Entities MapOfEntities;
struct MapOfEntitiesPairToPythonConverter {
static PyObject* convert(const MapOfEntities::value_type& pair) {
return bp::incref(bp::make_tuple(pair.first, bp::ptr(pair.second)).ptr());
}
};
MapOfEntities* getEntityMap() {
return const_cast<MapOfEntities*>(
&dg::PoolStorage::getInstance()->getEntityMap());
}
dg::SignalBase<int>* getSignal(dg::Entity& e, const std::string& name) {
return &e.getSignal(name);
}
class PythonEntity : public dg::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
using dg::Entity::Entity;
void signalRegistration(dg::SignalBase<int>& signal) {
dg::Entity::signalRegistration(signal);
}
void signalDeregistration(const std::string& name) {
dg::Entity::signalDeregistration(name);
}
};
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PythonEntity, "PythonEntity");
void exposeEntityBase() {
using namespace dynamicgraph;
bp::enum_<LoggerVerbosity>("LoggerVerbosity")
.value("VERBOSITY_ALL", VERBOSITY_ALL)
.value("VERBOSITY_INFO_WARNING_ERROR", VERBOSITY_INFO_WARNING_ERROR)
.value("VERBOSITY_WARNING_ERROR", VERBOSITY_WARNING_ERROR)
.value("VERBOSITY_ERROR", VERBOSITY_ERROR)
.value("VERBOSITY_NONE", VERBOSITY_NONE)
.export_values();
bp::class_<Entity, boost::noncopyable>("Entity", bp::no_init)
.add_property("name",
bp::make_function(
&Entity::getName,
bp::return_value_policy<bp::copy_const_reference>()))
.add_property("className",
bp::make_function(
&Entity::getClassName,
bp::return_value_policy<bp::copy_const_reference>()),
"the class name of the Entity")
.add_property("__doc__", &Entity::getDocString)
.def("setLoggerVerbosityLevel", &Entity::setLoggerVerbosityLevel)
.def("getLoggerVerbosityLevel", &Entity::getLoggerVerbosityLevel)
.add_property("loggerVerbosityLevel", &Entity::setLoggerVerbosityLevel,
&Entity::getLoggerVerbosityLevel,
"the verbosity level of the entity")
.def("setTimeSample", &Entity::setTimeSample)
.def("getTimeSample", &Entity::getTimeSample)
.add_property("timeSample", &Entity::getTimeSample,
&Entity::setTimeSample,
"the time sample for printing debugging information")
.def("setStreamPrintPeriod", &Entity::setStreamPrintPeriod)
.def("getStreamPrintPeriod", &Entity::getStreamPrintPeriod)
.add_property("streamPrintPeriod", &Entity::getStreamPrintPeriod,
&Entity::setStreamPrintPeriod,
"set the period at which debugging information are printed")
.def(
"__str__",
+[](const Entity& e) -> std::string {
std::ostringstream os;
e.display(os);
return os.str();
})
.def(
"signals",
+[](const Entity& e) -> bp::list {
bp::list ret;
for (auto& el : e.getSignalMap()) ret.append(bp::ptr(el.second));
return ret;
},
"Return the list of signals.")
//.def("signal", +[](Entity& e, const std::string &name) { return
//&e.getSignal(name); },
// reference_existing_object())
.def("signal", &getSignal, reference_existing_object(),
"get signal by name from an Entity", bp::arg("name"))
.def("hasSignal", &Entity::hasSignal,
"return True if the entity has a signal with the given name")
.def(
"displaySignals",
+[](const Entity& e) {
Entity::SignalMap signals(e.getSignalMap());
std::cout << "--- <" << e.getName();
if (signals.empty())
std::cout << "> has no signal\n";
else
std::cout << "> signal list:\n";
for (const auto& el : signals)
el.second->display(std::cout << " |-- <") << '\n';
},
"Print the list of signals into standard output: temporary.")
/*
.def("__getattr__", +[](Entity& e, const std::string &name) ->
SignalBase<int>* { return &e.getSignal(name); },
reference_existing_object())
def __getattr__(self, name):
try:
return self.signal(name)
except Exception:
try:
object.__getattr__(self, name)
except AttributeError:
raise AttributeError("'%s' entity has no attribute %s\n" %
(self.name, name) + ' entity attributes are usually either\n' + ' -
commands,\n' + ' - signals or,\n' + ' - user defined attributes')
*/
/*
.def("__setattr__", +[](bp::object self, const std::string &name,
bp::object value) { Entity& e = bp::extract<Entity&> (self); if
(e.hasSignal(name)) throw std::invalid_argument(name + " already
designates a signal. " "It is not advised to set a new attribute of the
same name.");
// TODO How do you do that ? I am sure it is possible.
//object.__setattr__(self, name, value)
})
*/
/* TODO ?
def boundNewCommand(self, cmdName):
"""
At construction, all existing commands are bound directly in the
class. This method enables to bound new commands dynamically. These new
bounds are not made with the class, but directly with the object instance.
"""
def boundAllNewCommands(self):
"""
For all commands that are not attribute of the object instance nor of
the class, a new attribute of the instance is created to bound the
command.
"""
*/
// For backward compat
.add_static_property(
"entities",
bp::make_function(&getEntityMap, reference_existing_object()));
python::exposeEntity<PythonEntity, bp::bases<Entity>, 0>()
.def("signalRegistration", &PythonEntity::signalRegistration)
.def("signalDeregistration", &PythonEntity::signalDeregistration);
python::exposeEntity<python::PythonSignalContainer, bp::bases<Entity>, 0>()
.def("rmSignal", &python::PythonSignalContainer::rmSignal,
"Remove a signal", bp::arg("signal_name"));
}
void exposeCommand() {
using dg::command::Command;
bp::class_<Command, boost::noncopyable>("Command", bp::no_init)
.def("__call__", bp::raw_function(dg::python::entity::executeCmd, 1),
"execute the command")
.add_property("__doc__", &Command::getDocstring);
}
void exposeOldAPI() {
bp::def("plug", dynamicgraph::python::plug,
"plug an output signal into an input signal",
(bp::arg("signalOut"), "signalIn"));
bp::def("enableTrace", dynamicgraph::python::enableTrace,
"Enable or disable tracing debug info in a file");
// Signals
bp::def("create_signal_wrapper",
dynamicgraph::python::signalBase::createSignalWrapper,
reference_existing_object(), "create a SignalWrapper C++ object");
// Entity
bp::def("factory_get_entity_class_list",
dynamicgraph::python::factory::getEntityClassList,
"return the list of entity classes");
bp::def("writeGraph", dynamicgraph::python::pool::writeGraph,
"Write the graph of entities in a filename.");
bp::def("get_entity_list", dynamicgraph::python::pool::getEntityList,
"return the list of instanciated entities");
bp::def("addLoggerFileOutputStream",
dynamicgraph::python::debug::addLoggerFileOutputStream,
"add a output file stream to the logger by filename");
bp::def("addLoggerCoutOutputStream",
dynamicgraph::python::debug::addLoggerCoutOutputStream,
"add std::cout as output stream to the logger");
bp::def("closeLoggerFileOutputStream",
dynamicgraph::python::debug::closeLoggerFileOutputStream,
"close all the loggers file output streams.");
bp::def("real_time_logger_destroy",
dynamicgraph::python::debug::realTimeLoggerDestroy,
"Destroy the real time logger.");
bp::def("real_time_logger_spin_once",
dynamicgraph::python::debug::realTimeLoggerSpinOnce,
"Destroy the real time logger.");
bp::def("real_time_logger_instance",
dynamicgraph::python::debug::realTimeLoggerInstance,
"Starts the real time logger.");
}
void enableEigenPy() {
eigenpy::enableEigenPy();
if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::Quaterniond>())
eigenpy::exposeQuaternion();
if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::AngleAxisd>())
eigenpy::exposeAngleAxis();
eigenpy::enableEigenPySpecific<Eigen::Matrix4d>();
}
BOOST_PYTHON_MODULE(wrap) {
enableEigenPy();
exposeOldAPI();
dg::python::exposeSignals();
exposeEntityBase();
exposeCommand();
typedef dg::PoolStorage::Entities MapOfEntities;
bp::class_<MapOfEntities>("MapOfEntities")
.def("__len__", &MapOfEntities::size)
.def(
"keys",
+[](const MapOfEntities& m) -> bp::tuple {
bp::list res;
for (const auto& el : m) res.append(el.first);
return bp::tuple(res);
})
.def(
"values",
+[](const MapOfEntities& m) -> bp::tuple {
bp::list res;
for (const auto& el : m) res.append(bp::ptr(el.second));
return bp::tuple(res);
})
.def("__getitem__",
static_cast<dg::Entity*& (MapOfEntities::*)(const std::string& k)>(
&MapOfEntities::at),
reference_existing_object())
.def(
"__setitem__", +[](MapOfEntities& m, const std::string& n,
dg::Entity* e) { m.emplace(n, e); })
.def("__iter__", bp::iterator<MapOfEntities>())
.def(
"__contains__",
+[](const MapOfEntities& m, const std::string& n) -> bool {
return m.count(n);
});
bp::to_python_converter<MapOfEntities::value_type,
MapOfEntitiesPairToPythonConverter>();
}
// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.
#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/value.h>
#include <iostream>
#include "dynamic-graph/python/convert-dg-to-py.hh"
#include "dynamic-graph/python/dynamic-graph-py.hh"
// Ignore "dereferencing type-punned pointer will break strict-aliasing rules"
// warnings on gcc caused by Py_RETURN_TRUE and Py_RETURN_FALSE.
#if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 2))
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
#endif
namespace bp = boost::python;
using dynamicgraph::Entity;
using dynamicgraph::Matrix;
using dynamicgraph::SignalBase;
using dynamicgraph::Vector;
using dynamicgraph::command::Command;
using dynamicgraph::command::Value;
namespace dynamicgraph {
namespace python {
using namespace convert;
namespace entity {
/// \param obj an Entity object
void addCommands(bp::object obj) {
Entity& entity = bp::extract<Entity&>(obj);
for (const auto& el : entity.getNewStyleCommandMap())
obj.attr(el.first.c_str()) = bp::object(bp::ptr(el.second));
}
/// \param obj an Entity object
void addSignals(bp::object obj) {
Entity& entity = bp::extract<Entity&>(obj);
for (const auto& el : entity.getSignalMap())
// obj.attr(el.first.c_str()) = bp::make_function(
//+[&entity,el]() { return &entity.getSignal(el.first); },
// bp::return_value_policy<bp::reference_existing_object>());
obj.attr(el.first.c_str()) = bp::object(bp::ptr(el.second));
}
/**
\brief Create an instance of Entity
*/
Entity* create(const char* className, const char* instanceName) {
Entity* obj = NULL;
/* Try to find if the corresponding object already exists. */
if (dynamicgraph::PoolStorage::getInstance()->existEntity(instanceName,
obj)) {
if (obj->getClassName() != className) {
throw std::invalid_argument("Found an object named " +
std::string(instanceName) +
",\n"
"but this object is of type " +
std::string(obj->getClassName()) +
" and not " + std::string(className));
}
} else /* If not, create a new object. */
{
obj = dynamicgraph::FactoryStorage::getInstance()->newEntity(
std::string(className), std::string(instanceName));
}
return obj;
}
bp::object executeCmd(bp::tuple args, bp::dict) {
Command& command = bp::template extract<Command&>(args[0]);
if (bp::len(args) != int(command.valueTypes().size() + 1))
// TODO Put expected and given number of args
throw std::out_of_range("Wrong number of arguments");
std::vector<Value> values;
values.reserve(command.valueTypes().size());
for (int i = 1; i < bp::len(args); ++i)
values.push_back(convert::toValue(args[i], command.valueTypes()[i - 1]));
command.setParameterValues(values);
return convert::fromValue(command.execute());
}
} // namespace entity
} // namespace python
} // namespace dynamicgraph
""" # Copyright (C) 2020 CNRS
Copyright (C) 2010 CNRS #
# Author: Florent Lamiraux, Nicolas Mansard
Author: Florent Lamiraux from __future__ import print_function
"""
import wrap, signal_base
entityClassNameList = []
def commandMethod(name, docstring) :
def method(self, *arg):
return wrap.entity_execute_command(self.obj, name, arg)
method.__doc__ = docstring
return method
def initEntity(self, name):
"""
Common constructor of Entity classes
"""
Entity.__init__(self, self.className, name)
if not self.__class__.commandCreated:
# Get list of commands of the Entity object
commands = wrap.entity_list_commands(self.obj)
# for each command, add a method with the name of the command
for command in commands:
docstring = wrap.entity_get_command_docstring(self.obj, command)
setattr(self.__class__, command, commandMethod(command, docstring))
self.__class__.commandCreated = True
def updateEntityClasses(dictionary):
cxx_entityList = wrap.factory_get_entity_class_list()
for e in filter(lambda x: not x in entityClassNameList, cxx_entityList):
class metacls(type):
def __new__(mcs, name, bases, dict):
return type.__new__(mcs, name, bases, dict)
# Create new class
a = metacls(e, (Entity,), {})
# Store class name in class member
a.className = e
# set class constructor
setattr(a, '__init__', initEntity)
# set class attribute to store whether command methods have been created
setattr(a, 'commandCreated', False)
#
# Store new class in dictionary with class name
dictionary[e] = a
# Store class name in local list
entityClassNameList.append(e)
class Entity (object) :
"""
This class binds dynamicgraph::Entity C++ class
"""
obj = None
def __init__(self, className, instanceName):
"""
Constructor: if not called by a child class, create and store a pointer
to a C++ Entity object.
"""
object.__setattr__(self, 'obj', wrap.create_entity(className, instanceName) )
@property
def name(self) :
return wrap.entity_get_name(self.obj)
def signal (self, name) :
"""
Get a signal of the entity from signal name
"""
signalPt = wrap.entity_get_signal(self.obj, name)
return signal_base.SignalBase("", signalPt)
def displaySignals(self) :
"""
Print the list of signals into standard output: temporary.
"""
wrap.entity_display_signals(self.obj)
def signals(self) :
"""
Alias of displaySignals to reduce verbosity. To be confirmed.
"""
self.displaySignals()
def commands(self):
"""
Return the list of commands.
"""
return wrap.entity_list_commands(self.obj)
def globalHelp(self):
"""
Print a short description of each command.
"""
for cstr in self.commands():
ctitle=cstr+':'
for i in range(len(cstr),15):
ctitle+=' '
for docstr in wrap.entity_get_command_docstring(self.obj,cstr).split('\n'):
if (len(docstr)>0) and (not docstr.isspace()):
print ctitle+"\t"+docstr
break
def help( self,comm=None ):
"""
With no arg, print the global help. With arg the name of
a specific command, print the help associated to the command.
"""
if comm is None:
self.globalHelp()
else:
print comm+":\n"+wrap.entity_get_command_docstring(self.obj,comm)
def __getattr__(self, name):
try:
return self.signal(name)
except:
object.__getattr__(self, name)
# for backward compat
from .wrap import LoggerVerbosity as VerbosityLevel # noqa
from .wrap import Entity # noqa
// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.
#include <dynamic-graph/factory.h>
#include <iostream>
#include "dynamic-graph/python/dynamic-graph-py.hh"
using dynamicgraph::Entity;
using dynamicgraph::ExceptionAbstract;
namespace dynamicgraph {
namespace python {
namespace factory {
/**
\brief Get name of entity
*/
bp::tuple getEntityClassList() {
std::vector<std::string> classNames;
dynamicgraph::FactoryStorage::getInstance()->listEntities(classNames);
return to_py_tuple(classNames.begin(), classNames.end());
}
} // namespace factory
} // namespace python
} // namespace dynamicgraph
// Copyright 2011, 2012, Florent Lamiraux, LAAS-CNRS.
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <vector>
#include "dynamic-graph/python/dynamic-graph-py.hh"
namespace dynamicgraph {
namespace python {
namespace pool {
void writeGraph(const char* filename) {
PoolStorage::getInstance()->writeGraph(filename);
}
const std::map<std::string, Entity*>* getEntityMap() {
return &PoolStorage::getInstance()->getEntityMap();
}
/**
\brief Get list of entities
*/
bp::list getEntityList() {
std::vector<std::string> entityNames;
bp::list res;
const PoolStorage::Entities& listOfEntities =
PoolStorage::getInstance()->getEntityMap();
for (const auto& el : listOfEntities) res.append(el.second->getName());
return res;
}
} // namespace pool
} // namespace python
} // namespace dynamicgraph
#include "dynamic-graph/python/python-compat.hh"
// Get any PyObject and get its str() representation as an std::string
std::string obj_to_str(PyObject* o) {
std::string ret;
PyObject* os;
#if PY_MAJOR_VERSION >= 3
os = PyObject_Str(o);
assert(os != NULL);
assert(PyUnicode_Check(os));
ret = PyUnicode_AsUTF8(os);
#else
os = PyObject_Unicode(o);
assert(os != NULL);
assert(PyUnicode_Check(os));
PyObject* oss = PyUnicode_AsUTF8String(os);
assert(oss != NULL);
ret = PyString_AsString(oss);
Py_DECREF(oss);
#endif
Py_DECREF(os);
return ret;
}
# Defines the following shortcuts:
# signal.name -> return the shortname
# signal -> display nicely the content of the signal
# signal(3) -> recompute the signal at time 3, and display nicely
# signal +1 -> increment the signal time of 1, recompute, and display.
# signal.deps -> display the graph dependancy up to the default value (3)
# signal.deps(6) -> same, but with depth = 6.
# entity -> same as print(entity)
# change the prompt to be '%'
from __future__ import print_function
# Changing prompt
import sys
from .entity import Entity
from .signal_base import SignalBase
# Enables shortcut "name"
def sig_short_name(self):
return self.getName().split(":")[-1]
setattr(SignalBase, "name", property(sig_short_name))
# Enables shortcuts "m"
# This code implements a pseudo function 'm' in the class signal_base,
# with no args, or optional args. Three calls can be made:
# - sig.m : print the current value.
# - sig.m(time): recompute at given <time>, and display the current value
# - sig.m +time: recompute at <time> after current time, and display.
class PrettySignalPrint:
sig = None
def __init__(self, sig):
self.sig = sig
def __str__(self):
return self.sig.name + " = " + str(self.sig.value)
def __repr__(self):
return str(self)
def __call__(self, iter):
self.sig.recompute(iter)
return self
def __add__(self, iter):
self.sig.recompute(self.sig.time + iter)
return self
def sigMatPrint(sig):
return PrettySignalPrint(sig)
setattr(SignalBase, "m", property(PrettySignalPrint))
# Enable the same as 'm', but directly on the signal object.
def sigRepr(self):
return self.name + " = " + str(self.value)
def sigCall(sig, iter):
sig.recompute(iter)
print(sigRepr(sig))
def sigTimeIncr(sig, iter):
sig.recompute(sig.time + iter)
print(sigRepr(sig))
setattr(SignalBase, "__repr__", sigRepr)
setattr(SignalBase, "__call__", sigCall)
setattr(SignalBase, "__add__", sigTimeIncr)
# Enables shortcut "deps"
# Implements the peudo function 'deps', that can be called without arg,
# or specifying a specific depth to be printed.
class SignalDepPrint:
defaultDepth = 2
sig = None
def __init__(self, sig):
self.sig = sig
def __repr__(self):
return self.sig.displayDependencies(self.defaultDepth)
def __call__(self, depth):
self.defaultDepth = depth
return self
setattr(SignalBase, "deps", property(SignalDepPrint))
setattr(Entity, "sigs", property(Entity.displaySignals))
setattr(Entity, "__repr__", Entity.__str__)
sys.ps1 = "% "
# Enable function that can be call without()def optionalparentheses(f):
def optionalparentheses(f):
class decoclass:
def __init__(self, f):
self.functor = f
def __repr__(self):
res = self.functor()
if isinstance(res, str):
return res
else:
return ""
def __call__(self, *arg):
return self.functor(*arg)
return decoclass(f)
// Copyright 2010, Florent Lamiraux, Thomas Moulard, LAAS-CNRS.
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <dynamic-graph/signal.h>
#include <dynamic-graph/value.h>
#include <boost/python.hpp>
#include <iostream>
#include <sstream>
#include "dynamic-graph/python/dynamic-graph-py.hh"
#include "dynamic-graph/python/signal-wrapper.hh"
#include "dynamic-graph/python/signal.hh"
using dynamicgraph::SignalBase;
namespace bp = boost::python;
namespace dynamicgraph {
namespace python {
typedef int time_type;
typedef Eigen::AngleAxis<double> VectorUTheta;
typedef Eigen::Quaternion<double> Quaternion;
typedef Eigen::VectorXd Vector;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, 3, 3> MatrixRotation;
typedef Eigen::Matrix<double, 4, 4> Matrix4;
typedef Eigen::Transform<double, 3, Eigen::Affine> MatrixHomogeneous;
typedef Eigen::Matrix<double, 6, 6> MatrixTwist;
template <typename Time>
void exposeSignalBase(const char* name) {
typedef SignalBase<Time> S_t;
bp::class_<S_t, boost::noncopyable>(name, bp::no_init)
.add_property("time",
bp::make_function(
&S_t::getTime,
bp::return_value_policy<bp::copy_const_reference>()),
&S_t::setTime)
.add_property("name",
bp::make_function(
&S_t::getName,
bp::return_value_policy<bp::copy_const_reference>()))
.def("getName", &S_t::getName,
bp::return_value_policy<bp::copy_const_reference>())
.def(
"getClassName",
+[](const S_t& s) -> std::string {
std::string ret;
s.getClassName(ret);
return ret;
})
.def("plug", &S_t::plug, "Plug the signal to another signal")
.def("unplug", &S_t::unplug, "Unplug the signal")
.def("isPlugged", &S_t::isPlugged, "Whether the signal is plugged")
.def("getPlugged", &S_t::getPluged,
bp::return_value_policy<bp::reference_existing_object>(),
"To which signal the signal is plugged")
.def("recompute", &S_t::recompute, "Recompute the signal at given time")
.def(
"__str__",
+[](const S_t& s) -> std::string {
std::ostringstream oss;
s.display(oss);
return oss.str();
})
.def(
"displayDependencies",
+[](const S_t& s, int time) -> std::string {
std::ostringstream oss;
s.displayDependencies(oss, time);
return oss.str();
},
"Print the signal dependencies in a string");
}
template <>
auto exposeSignal<MatrixHomogeneous, time_type>(const std::string& name) {
typedef Signal<MatrixHomogeneous, time_type> S_t;
bp::class_<S_t, bp::bases<SignalBase<time_type> >, boost::noncopyable> obj(
name.c_str(), bp::init<std::string>());
obj.add_property(
"value",
+[](const S_t& signal) -> Matrix4 {
return signal.accessCopy().matrix();
},
+[](S_t& signal, const Matrix4& v) {
// TODO it isn't hard to support pinocchio::SE3 type here.
// However, this adds a dependency to pinocchio.
signal.setConstant(MatrixHomogeneous(v));
},
"the signal value.");
return obj;
}
void exposeSignals() {
exposeSignalBase<time_type>("SignalBase");
exposeSignalsOfType<bool, time_type>("Bool");
exposeSignalsOfType<int, time_type>("Int");
exposeSignalsOfType<double, time_type>("Double");
exposeSignalsOfType<Vector, time_type>("Vector");
exposeSignalsOfType<Vector3, time_type>("Vector3");
exposeSignalsOfType<Vector7, time_type>("Vector7");
exposeSignalsOfType<Matrix, time_type>("Matrix");
exposeSignalsOfType<MatrixRotation, time_type>("MatrixRotation");
exposeSignalsOfType<MatrixHomogeneous, time_type>("MatrixHomogeneous");
exposeSignalsOfType<MatrixTwist, time_type>("MatrixTwist");
exposeSignalsOfType<Quaternion, time_type>("Quaternion");
exposeSignalsOfType<VectorUTheta, time_type>("VectorUTheta");
}
namespace signalBase {
template <class T>
SignalWrapper<T, int>* createSignalWrapperTpl(const char* name, bp::object o,
std::string& error) {
typedef SignalWrapper<T, int> SignalWrapper_t;
if (!SignalWrapper_t::checkCallable(o, error)) {
return NULL;
}
SignalWrapper_t* obj = new SignalWrapper_t(name, o);
return obj;
}
PythonSignalContainer* getPythonSignalContainer() {
Entity* obj = entity::create("PythonSignalContainer", "python_signals");
return dynamic_cast<PythonSignalContainer*>(obj);
}
#define SIGNAL_WRAPPER_TYPE(IF, Enum, Type) \
IF(command::Value::typeName(command::Value::Enum).compare(type) == 0) { \
obj = createSignalWrapperTpl<Type>(name, object, error); \
}
/**
\brief Create an instance of SignalWrapper
*/
SignalBase<int>* createSignalWrapper(const char* name, const char* type,
bp::object object) {
PythonSignalContainer* psc = getPythonSignalContainer();
if (psc == NULL) return NULL;
SignalBase<int>* obj = NULL;
std::string error;
SIGNAL_WRAPPER_TYPE(if, BOOL, bool)
// SIGNAL_WRAPPER_TYPE(else if, UNSIGNED ,bool)
SIGNAL_WRAPPER_TYPE(else if, INT, int)
SIGNAL_WRAPPER_TYPE(else if, FLOAT, float)
SIGNAL_WRAPPER_TYPE(else if, DOUBLE, double)
// SIGNAL_WRAPPER_TYPE(else if, STRING ,bool)
SIGNAL_WRAPPER_TYPE(else if, VECTOR, Vector)
// SIGNAL_WRAPPER_TYPE(else if, MATRIX ,bool)
// SIGNAL_WRAPPER_TYPE(else if, MATRIX4D ,bool)
else {
error = "Type not understood";
}
if (obj == NULL) throw std::runtime_error(error);
// Register signal into the python signal container
psc->signalRegistration(*obj);
// Return the pointer
return obj;
}
} // namespace signalBase
} // namespace python
} // namespace dynamicgraph
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
#include "dynamic-graph/python/signal-wrapper.hh"
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/factory.h>
namespace dynamicgraph {
namespace python {
void PythonSignalContainer::signalRegistration(
const SignalArray<int>& signals) {
Entity::signalRegistration(signals);
}
void PythonSignalContainer::rmSignal(const std::string& name) {
Entity::signalDeregistration(name);
}
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PythonSignalContainer,
"PythonSignalContainer");
template <class T, class Time>
bool SignalWrapper<T, Time>::checkCallable(pyobject c, std::string& error) {
if (PyCallable_Check(c.ptr()) == 0) {
error = boost::python::extract<std::string>(c.attr("__str__")());
error += " is not callable";
return false;
}
return true;
}
template class SignalWrapper<bool, int>;
template class SignalWrapper<int, int>;
template class SignalWrapper<float, int>;
template class SignalWrapper<double, int>;
template class SignalWrapper<Vector, int>;
} // namespace python
} // namespace dynamicgraph
""" # Copyright (C) 2020 CNRS
Copyright (C) 2010 CNRS #
# Author: Joseph Mirabel
Author: Florent Lamiraux from __future__ import print_function
"""
import wrap # I kept what follows for backward compatibility but I think it should be
import entity # removed
import re import re
def stringToTuple (vector) : from .wrap import SignalBase # noqa
from .wrap import create_signal_wrapper as SignalWrapper # noqa
def stringToTuple(vector):
""" """
Transform a string of format '[n](x_1,x_2,...,x_n)' into a tuple of numbers. Transform a string of format '[n](x_1,x_2,...,x_n)' into a tuple of numbers.
""" """
# Find vector length # Find vector length
a = re.match('\[(\d+)\]',vector) a = re.match(r"\[(\d+)\]", vector)
size = int(a.group(1)) size = int(a.group(1))
# remove '[n]' prefix # remove '[n]' prefix
vector = vector[len(a.group(0)):] vector = vector[len(a.group(0)) :]
# remove '(' and ')' at beginning and end # remove '(' and ')' at beginning and end
vector = vector.lstrip('(').rstrip(')\n') vector = vector.lstrip("(").rstrip(")\n")
# split string by ',' # split string by ','
vector = vector.split(',') vector = vector.split(",")
# check size # check size
if len(vector) != size: if len(vector) != size:
raise TypeError('displayed size ' + raise TypeError(
size + ' of vector does not fit actual size: ' "displayed size "
+ str(len(vector))) + str(size)
+ " of vector does not fit actual size: "
+ str(len(vector))
)
res = map(float, vector) res = map(float, vector)
return tuple (res) return tuple(res)
def tupleToString (vector) :
def tupleToString(vector):
""" """
Transform a tuple of numbers into a string of format Transform a tuple of numbers into a string of format
'[n](x_1, x_2, ..., x_n)' '[n](x_1, x_2, ..., x_n)'
""" """
string = '[%d]('%len (vector) string = "[%d](" % len(vector)
for x in vector[:-1]: for x in vector[:-1]:
string += '%f,'%x string += "%f," % x
string += '%f)'%vector[-1] string += "%f)" % vector[-1]
return string return string
def stringToMatrix (string) :
def stringToMatrix(string):
""" """
Transform a string of format Transform a string of format
'[n,m]((x_11,x_12,...,x_1m),...,(x_n1,x_n2,...,x_nm))' into a tuple '[n,m]((x_11,x_12,...,x_1m),...,(x_n1,x_n2,...,x_nm))' into a tuple
of tuple of numbers. of tuple of numbers.
""" """
# Find matrix size # Find matrix size
a = re.search ('\[(\d+),(\d+)]', string) a = re.search(r"\[(\d+),(\d+)]", string)
nRows = int (a.group(1)) nRows = int(a.group(1))
nCols = int (a.group(2)) nCols = int(a.group(2))
# Remove '[n,m]' prefix # Remove '[n,m]' prefix
string = string[len(a.group(0)):] string = string[len(a.group(0)) :]
rows = string.split('),(') rows = string.split("),(")
if len(rows) != nRows: if len(rows) != nRows:
raise TypeError('displayed nb rows ' + raise TypeError(
nRows + ' of matrix does not fit actual nb rows: ' "displayed nb rows "
+ str(len(rows))) + nRows
+ " of matrix does not fit actual nb rows: "
+ str(len(rows))
)
m = [] m = []
for rstr in rows: for rstr in rows:
rstr = rstr.lstrip('(').rstrip(')\n') rstr = rstr.lstrip("(").rstrip(")\n")
r = map(float, rstr.split(',')) r = map(float, rstr.split(","))
if len(r) != nCols: if len(r) != nCols:
raise TypeError('one row length ' + raise TypeError(
len(r) + "one row length "
' of matrix does not fit displayed nb cols: ' + + len(r)
nCols) + " of matrix does not fit displayed nb cols: "
+ nCols
)
m.append(tuple(r)) m.append(tuple(r))
return tuple (m) return tuple(m)
def matrixToString(matrix) : def matrixToString(matrix):
""" """
Transform a tuple of tuple of numbers into a string of format Transform a tuple of tuple of numbers into a string of format
'[n,m]((x_11,x_12,...,x_1m),...,(x_n1,x_n2,...,x_nm))'. '[n,m]((x_11,x_12,...,x_1m),...,(x_n1,x_n2,...,x_nm))'.
""" """
nRows = len(matrix) nRows = len(matrix)
if nRows is 0 : if nRows == 0:
return '[0,0](())' return "[0,0](())"
nCols = len(matrix[0]) nCols = len(matrix[0])
string = '[%d,%d](' % (nRows, nCols) string = "[%d,%d](" % (nRows, nCols)
for r in range (nRows) : for r in range(nRows):
string += '(' string += "("
for c in range (nCols) : for c in range(nCols):
string += str (float (matrix [r][c])) string += str(float(matrix[r][c]))
if c != nCols-1 : if c != nCols - 1:
string += ',' string += ","
string += ')' string += ")"
if r != nRows -1 : if r != nRows - 1:
string += ',' string += ","
string += ')' string += ")"
return string return string
def objectToString(obj) :
def objectToString(obj):
""" """
Transform an object to a string. Object is either Transform an object to a string. Object is either
- an entity (more precisely a sub-class named Feature) - an entity (more precisely a sub-class named Feature)
...@@ -100,23 +117,24 @@ def objectToString(obj) : ...@@ -100,23 +117,24 @@ def objectToString(obj) :
- an integer, - an integer,
- a boolean, - a boolean,
""" """
if (isinstance(obj, tuple)) : if hasattr(obj, "__iter__"):
# matrix or vector # matrix or vector
if len(obj) is 0 : if len(obj) == 0:
return "" return ""
else : else:
if (isinstance(obj[0], tuple)) : if hasattr(obj[0], "__iter__"):
#matrix # matrix
return matrixToString(obj) return matrixToString(obj)
else : else:
#vector # vector
return tupleToString(obj) return tupleToString(obj)
elif isinstance(obj, entity.Entity) : elif hasattr(obj, "name"):
return obj.name return obj.name
else : else:
return str(obj) return str(obj)
def stringToObject(string) :
def stringToObject(string):
""" """
Convert a string into one of the following types Convert a string into one of the following types
- a matrix (tuple of tuple), - a matrix (tuple of tuple),
...@@ -126,88 +144,25 @@ def stringToObject(string) : ...@@ -126,88 +144,25 @@ def stringToObject(string) :
Successively attempts conversion in the above order and return Successively attempts conversion in the above order and return
on success. If no conversion fits, the string is returned. on success. If no conversion fits, the string is returned.
""" """
try : if isinstance(string, float):
return string
if isinstance(string, int):
return string
if isinstance(string, tuple):
return string
try:
return stringToMatrix(string) return stringToMatrix(string)
except : except Exception:
pass pass
try : try:
return stringToTuple(string) return stringToTuple(string)
except : except Exception:
pass pass
try : try:
return int(string) return int(string)
except : except Exception:
pass pass
try : try:
return float(string) return float(string)
except : except Exception:
return string return string
class SignalBase (object) :
"""
This class binds dynamicgraph::SignalBase<int> C++ class
"""
obj = None
def __init__(self, name, obj = None) :
"""
Constructor: if not called by a child class, create and store a pointer
to a C++ SignalBase<int> object.
"""
if obj :
self.obj = obj
if not self.obj :
self.obj = wrap.create_signal_base(self, name)
@property
def time(self) :
"""
Get time of signal
"""
return wrap.signal_base_get_time(self.obj)
@property
def value(self) :
"""
Setter and getter for the value of a signal
Binds C++ SignalBase<int>::get() and set() methods. Values are passed
through string streams.
A string is interpreted as respectively:
* a matrix (tuple of tuple) if string fits '[n,m]((x_11,x_12,...,x_1m),...,(x_n1,x_n2,...,x_nm))' format where n and m are integers, x_ij are floating point numbers,
* a tuple if string fits '[n](x_1, x_2, ..., x_n)' format,
* an integer,
* a floating point number.
If string fits none of the above formats, no conversion is performed.
For instance, is s binds a signal of type vector,
>>> s.value = (2.5, .1, 1e2)
will call SignalBase<int>::set("[3](2.5,0.1,100.0)") and
>>> s.value
(2.5, 0.1, 100.0)
"""
string = wrap.signal_base_get_value(self.obj)
return stringToObject(string)
@value.setter
def value(self, val) :
"""
Set the signal as a constant signal with given value.
If the signal is plugged, it will be unplugged
"""
string = objectToString(val)
return wrap.signal_base_set_value(self.obj, string)
def getName(self):
"""
Get name of signal
"""
return wrap.signal_base_get_name(self.obj)
def recompute(self, time):
"""
Force signal to recompute the value at given time.
"""
return wrap.signal_base_recompute(self.obj, time)
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
from __future__ import print_function
def addTrace(robot, trace, entityName, signalName, autoRecompute=True):
"""
Add a signal to a tracer and recompute it automatically if necessary.
"""
signal = "{0}.{1}".format(entityName, signalName)
filename = "{0}-{1}".format(entityName, signalName).replace("/", "_")
trace.add(signal, filename)
if autoRecompute:
robot.device.after.addSignal(signal)
#include <dynamic-graph/tracer.h>
#include "dynamic-graph/python/module.hh"
BOOST_PYTHON_MODULE(wrap) {
using dynamicgraph::Tracer;
bp::import("dynamic_graph");
dynamicgraph::python::exposeEntity<Tracer>().def("addSignal",
&Tracer::addSignalToTrace);
}
#include <dynamic-graph/tracer-real-time.h>
#include "dynamic-graph/python/module.hh"
BOOST_PYTHON_MODULE(wrap) {
using dynamicgraph::Tracer;
using dynamicgraph::TracerRealTime;
bp::import("dynamic_graph.tracer");
dynamicgraph::python::exposeEntity<TracerRealTime, bp::bases<Tracer> >();
}
/*
* Copyright 2010 (C) CNRS
* Author: Florent Lamiraux
*/
#include <Python.h>
#include <iostream>
//#include <sstream>
//#include <string>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/linear-algebra.h>
#include "dynamic-graph/command.h"
#include "dynamic-graph/value.h"
using dynamicgraph::Entity;
using dynamicgraph::SignalBase;
using dynamicgraph::ExceptionAbstract;
using dynamicgraph::command::Command;
using dynamicgraph::command::Value;
using dynamicgraph::Vector;
using dynamicgraph::Matrix;
namespace dynamicgraph {
namespace python {
extern PyObject* error;
namespace entity {
static void destroy (void* self);
static void fillMatrixRow(Matrix& m, unsigned index,
PyObject* tuple);
static Value pythonToValue(PyObject* pyObject,
const Value::Type& valueType);
static PyObject* vectorToPython(const Vector& vector);
static PyObject* valueToPython(const Value& value);
/**
\brief Create an instance of Entity
*/
PyObject* create(PyObject* self, PyObject* args)
{
char *className = NULL;
char *instanceName = NULL;
if (!PyArg_ParseTuple(args, "ss", &className, &instanceName))
return NULL;
Entity* obj = NULL;
try {
obj = dynamicgraph::g_factory.newEntity(std::string(className),
std::string(instanceName));
} catch (std::exception& exc) {
PyErr_SetString(error, exc.what());
return NULL;
}
// Return the pointer as a PyCObject
return PyCObject_FromVoidPtr((void*)obj, NULL);
}
/**
\brief Destroy an instance of Entity
*/
static void destroy (void* self)
{
Entity* obj = (Entity*)self;
delete obj;
}
/**
\brief Get name of entity
*/
PyObject* getName(PyObject* self, PyObject* args)
{
PyObject* object = NULL;
void* pointer = NULL;
std::string name;
if (!PyArg_ParseTuple(args, "O", &object))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
Entity* entity = (Entity*)pointer;
try {
name = entity->getName();
} catch(ExceptionAbstract& exc) {
PyErr_SetString(error, exc.getStringMessage().c_str());
return NULL;
}
return Py_BuildValue("s", name.c_str());
}
/**
\brief Get a signal by name
*/
PyObject* getSignal(PyObject* self, PyObject* args)
{
char *name = NULL;
PyObject* object = NULL;
void* pointer = NULL;
if (!PyArg_ParseTuple(args, "Os", &object, &name))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
Entity* entity = (Entity*)pointer;
SignalBase<int>* signal = NULL;
try {
signal = &(entity->getSignal(std::string(name)));
} catch(ExceptionAbstract& exc) {
PyErr_SetString(error, exc.getStringMessage().c_str());
return NULL;
}
// Return the pointer to the signal without destructor since the signal
// is not owned by the calling object but by the Entity.
return PyCObject_FromVoidPtr((void*)signal, NULL);
}
PyObject* displaySignals(PyObject* self, PyObject* args)
{
void* pointer = NULL;
PyObject* object = NULL;
if (!PyArg_ParseTuple(args, "O", &object))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
Entity* entity = (Entity*)pointer;
try {
entity->displaySignalList(std::cout);
} catch(ExceptionAbstract& exc) {
PyErr_SetString(error, exc.getStringMessage().c_str());
return NULL;
}
return Py_BuildValue("");
}
void fillMatrixRow(Matrix& m, unsigned iRow, PyObject* tuple)
{
if (PyTuple_Size(tuple) != m.nbCols()) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"lines of matrix have different sizes.");
}
for (unsigned int iCol=0; iCol < m.nbCols(); iCol++) {
PyObject* pyDouble = PyTuple_GetItem(tuple, iCol);
if (!PyFloat_Check(pyDouble)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"element of matrix should be "
"a floating point number.");
}
m(iRow, iCol) = PyFloat_AsDouble(pyDouble);
}
}
Value pythonToValue(PyObject* pyObject,
const Value::Type& valueType)
{
bool bvalue;
unsigned uvalue;
int ivalue;
float fvalue;
double dvalue;
std::string svalue;
Vector v;
Matrix m;
unsigned int nCols;
unsigned int size;
PyObject* row;
unsigned int nRows;
switch (valueType) {
case (Value::BOOL) :
if (!PyBool_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"bool");
}
bvalue = (pyObject == Py_True);
return Value(bvalue);
break;
case (Value::UNSIGNED) :
if (!PyInt_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"unsigned int");
}
uvalue = PyInt_AsUnsignedLongMask(pyObject);
return Value(uvalue);
break;
case (Value::INT) :
if (!PyInt_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"int");
}
ivalue = (int)PyInt_AS_LONG(pyObject);
return Value(ivalue);
break;
case (Value::FLOAT) :
if (!PyFloat_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"float");
}
fvalue = (float)PyFloat_AsDouble(pyObject);
return Value(fvalue);
break;
case (Value::DOUBLE) :
if (!PyFloat_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"float");
}
dvalue = PyFloat_AsDouble(pyObject);
return Value(dvalue);
break;
case (Value::STRING) :
if (!PyString_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"string");
}
svalue = PyString_AsString(pyObject);
return Value(svalue);
break;
case (Value::VECTOR) :
// Check that argument is a tuple
if (!PyTuple_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"vector");
}
size = PyTuple_Size(pyObject);
v.resize(size);
for (unsigned int i=0; i<size; i++) {
PyObject* pyDouble = PyTuple_GetItem(pyObject, i);
if (!PyFloat_Check(pyDouble)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"element of vector should be a floating "
"point number.");
}
v(i) = PyFloat_AsDouble(pyDouble);
}
return Value(v);
break;
case (Value::MATRIX) :
// Check that argument is a tuple
if (!PyTuple_Check(pyObject)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"matrix");
}
nRows = PyTuple_Size(pyObject);
if (nRows == 0) {
return Value(Matrix());
}
row = PyTuple_GetItem(pyObject, 0);
if (!PyTuple_Check(row)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"matrix");
}
nCols = PyTuple_Size(row);
m.resize(nRows, nCols);
fillMatrixRow(m, 0, row);
for (unsigned int iRow=1; iRow<nRows; iRow++) {
row = PyTuple_GetItem(pyObject, iRow);
if (!PyTuple_Check(row)) {
throw ExceptionFactory(ExceptionFactory::GENERIC,
"matrix");
}
fillMatrixRow(m, iRow, row);
}
return Value(m);
break;
default:
std::cerr << "Only int, double and string are supported."
<< std::endl;
}
return Value();
}
PyObject* vectorToPython(const Vector& vector)
{
PyObject* tuple = PyTuple_New(vector.size());
for (unsigned int index = 0; index < vector.size(); index++) {
PyObject* pyDouble = PyFloat_FromDouble(vector(index));
PyTuple_SET_ITEM(tuple, index, pyDouble);
}
return tuple;
}
PyObject* matrixToPython(const Matrix& matrix)
{
PyObject* tuple = PyTuple_New(matrix.nbRows());
for (unsigned int iRow = 0; iRow < matrix.nbRows(); iRow++) {
PyObject* row = PyTuple_New(matrix.nbCols());
for (unsigned iCol=0; iCol < matrix.nbCols(); iCol++) {
PyObject* pyDouble = PyFloat_FromDouble(matrix(iRow, iCol));
PyTuple_SET_ITEM(row, iCol, pyDouble);
}
PyTuple_SET_ITEM(tuple, iRow, row);
}
return tuple;
}
PyObject* valueToPython(const Value& value)
{
bool boolValue;
unsigned unsignedValue;
int intValue;
float floatValue;
double doubleValue;
std::string stringValue;
Vector vectorValue;
Matrix matrixValue;
switch(value.type()) {
case (Value::BOOL) :
boolValue = value.value();
if (boolValue) {
return PyBool_FromLong(1);
}
return PyBool_FromLong(0);
case (Value::UNSIGNED) :
unsignedValue = value.value();
return Py_BuildValue("I", unsignedValue);
case (Value::INT) :
intValue = value.value();
return Py_BuildValue("i", intValue);
case (Value::FLOAT) :
floatValue = value.value();
return Py_BuildValue("f", floatValue);
case (Value::DOUBLE) :
doubleValue = value.value();
return Py_BuildValue("d", doubleValue);
case (Value::STRING) :
stringValue = (std::string) value.value();
return Py_BuildValue("s", stringValue.c_str());
case (Value::VECTOR) :
vectorValue = value.value();
return vectorToPython(vectorValue);
case (Value::MATRIX) :
matrixValue = value.value();
return matrixToPython(matrixValue);
default:
return Py_BuildValue("");
}
return Py_BuildValue("");
}
PyObject* executeCommand(PyObject* self, PyObject* args)
{
PyObject* object = NULL;
PyObject* argTuple = NULL;
char* commandName = NULL;
void* pointer = NULL;
if (!PyArg_ParseTuple(args, "OsO", &object, &commandName, &argTuple)) {
return NULL;
}
// Retrieve the entity instance
if (!PyCObject_Check(object)) {
PyErr_SetString(error, "first argument is not an object");
return NULL;
}
pointer = PyCObject_AsVoidPtr(object);
Entity* entity = (Entity*)pointer;
// Retrieve the argument tuple
if (!PyTuple_Check(argTuple)) {
PyErr_SetString(error, "third argument is not a tuple");
return NULL;
}
unsigned int size = PyTuple_Size(argTuple);
std::map<const std::string, Command*> commandMap =
entity->getNewStyleCommandMap();
if (commandMap.count(std::string(commandName)) != 1) {
std::string msg = "command " + std::string(commandName) +
" is not referenced in Entity " + entity->getName();
PyErr_SetString(error, msg.c_str());
return NULL;
}
Command* command = commandMap[std::string(commandName)];
// Check that tuple size is equal to command number of arguments
const std::vector<Value::Type> typeVector = command->valueTypes();
if (size != typeVector.size()) {
std::stringstream ss;
ss << "command takes " << typeVector.size()
<< " parameters, " << size << " given.";
PyErr_SetString(error, ss.str().c_str());
return NULL;
}
std::vector<Value> valueVector;
for (unsigned int iParam=0; iParam<size; iParam++) {
PyObject* PyValue = PyTuple_GetItem(argTuple, iParam);
Value::Type valueType = typeVector[iParam];
try {
Value value = pythonToValue(PyValue, valueType);
valueVector.push_back(value);
} catch (ExceptionAbstract& exc) {
std::stringstream ss;
ss << "argument " << iParam+1 << " should be a "
<< exc.what() << ".";
PyErr_SetString(error, ss.str().c_str()) ;
return NULL;
}
}
command->setParameterValues(valueVector);
try {
Value result = command->execute();
return valueToPython(result);
} catch (const std::exception& exc) {
PyErr_SetString(error, exc.what()) ;
return NULL;
}
return NULL;
}
PyObject* listCommands(PyObject* self, PyObject* args)
{
PyObject* object = NULL;
if (!PyArg_ParseTuple(args, "O", &object)) {
return NULL;
}
// Retrieve the entity instance
if (!PyCObject_Check(object)) {
PyErr_SetString(error, "first argument is not an object");
return NULL;
}
void* pointer = PyCObject_AsVoidPtr(object);
Entity* entity = (Entity*)pointer;
typedef std::map<const std::string, command::Command*> CommandMap;
CommandMap map = entity->getNewStyleCommandMap();
unsigned int nbCommands = map.size();
// Create a tuple of same size as the command map
PyObject* result = PyTuple_New(nbCommands);
unsigned int count = 0;
for (CommandMap::iterator it=map.begin();
it != map.end(); it++) {
std::string commandName = it->first;
PyObject* pyName = Py_BuildValue("s", commandName.c_str());
PyTuple_SET_ITEM(result, count, pyName);
count++;
}
return result;
}
PyObject* getCommandDocstring(PyObject* self, PyObject* args)
{
PyObject* object = NULL;
char* commandName;
if (!PyArg_ParseTuple(args, "Os", &object, &commandName)) {
return NULL;
}
// Retrieve the entity instance
if (!PyCObject_Check(object)) {
PyErr_SetString(error, "first argument is not an object");
return NULL;
}
void* pointer = PyCObject_AsVoidPtr(object);
Entity* entity = (Entity*)pointer;
typedef std::map<const std::string, command::Command*> CommandMap;
CommandMap map = entity->getNewStyleCommandMap();
command::Command* command = NULL;
try {
command = map[commandName];
} catch (const std::exception& exc) {
PyErr_SetString(error, exc.what());
}
std::string docstring = command->getDocstring();
return Py_BuildValue("s", docstring.c_str());
}
}
}
}
/*
* Copyright 2010 (C) CNRS
* Author: Florent Lamiraux
*/
#include <Python.h>
#include <iostream>
#include <dynamic-graph/factory.h>
using dynamicgraph::Entity;
using dynamicgraph::ExceptionAbstract;
namespace dynamicgraph {
namespace python {
extern PyObject* error;
namespace factory {
/**
\brief Get name of entity
*/
PyObject* getEntityClassList(PyObject* self, PyObject* args)
{
if (!PyArg_ParseTuple(args, ""))
return NULL;
std::vector <std::string> classNames;
dynamicgraph::g_factory.listEntities(classNames);
unsigned int classNumber = classNames.size();
// Build a tuple object
PyObject* classTuple = PyTuple_New(classNumber);
for (unsigned int iEntity = 0; iEntity < classNames.size(); iEntity++) {
PyObject* className = Py_BuildValue("s", classNames[iEntity].c_str());
PyTuple_SetItem(classTuple, iEntity, className);
}
return Py_BuildValue("O", classTuple);
}
} // namespace factory
} // namespace python
} // namespace dynamicgraph
// -*- mode: c++ -*-
// Copyright 2011, Florent Lamiraux, CNRS.
#ifdef WIN32
#include <Windows.h>
#else
#include <dlfcn.h>
#endif
#include <iostream>
#include "dynamic-graph/debug.h"
#include "dynamic-graph/python/interpreter.hh"
std::ofstream dg_debugfile("/tmp/dynamic-graph-traces.txt",
std::ios::trunc& std::ios::out);
// Python initialization commands
namespace dynamicgraph {
namespace python {
static const std::string pythonPrefix[8] = {
"from __future__ import print_function\n",
"import traceback\n",
"class StdoutCatcher:\n"
" def __init__(self):\n"
" self.data = ''\n"
" def write(self, stuff):\n"
" self.data = self.data + stuff\n"
" def fetch(self):\n"
" s = self.data[:]\n"
" self.data = ''\n"
" return s\n",
"stdout_catcher = StdoutCatcher()\n",
"stderr_catcher = StdoutCatcher()\n",
"import sys\n",
"sys.stdout = stdout_catcher",
"sys.stderr = stderr_catcher"};
bool HandleErr(std::string& err, PyObject* globals_, int PythonInputType) {
dgDEBUGIN(15);
err = "";
bool lres = false;
if (PyErr_Occurred() != NULL) {
bool is_syntax_error = PyErr_ExceptionMatches(PyExc_SyntaxError);
PyErr_Print();
PyObject* stderr_obj = PyRun_String("stderr_catcher.fetch()", Py_eval_input,
globals_, globals_);
err = obj_to_str(stderr_obj);
Py_DECREF(stderr_obj);
// Here if there is a syntax error and
// and the interpreter input is set to Py_eval_input,
// it is maybe a statement instead of an expression.
// Therefore we indicate to re-evaluate the command.
if (is_syntax_error && PythonInputType == Py_eval_input) {
dgDEBUG(15) << "Detected a syntax error " << std::endl;
lres = false;
} else
lres = true;
PyErr_Clear();
} else {
dgDEBUG(15) << "no object generated but no error occured." << std::endl;
}
return lres;
}
Interpreter::Interpreter() {
// load python dynamic library
// this is silly, but required to be able to import dl module.
#ifndef WIN32
dlopen(PYTHON_LIBRARY, RTLD_LAZY | RTLD_GLOBAL);
#endif
Py_Initialize();
#if PY_MAJOR_VERSION < 3 || PY_MINOR_VERSION < 7
PyEval_InitThreads();
#endif
mainmod_ = PyImport_AddModule("__main__");
Py_INCREF(mainmod_);
globals_ = PyModule_GetDict(mainmod_);
assert(globals_);
Py_INCREF(globals_);
PyRun_SimpleString(pythonPrefix[0].c_str());
PyRun_SimpleString(pythonPrefix[1].c_str());
PyRun_SimpleString(pythonPrefix[2].c_str());
PyRun_SimpleString(pythonPrefix[3].c_str());
PyRun_SimpleString(pythonPrefix[4].c_str());
PyRun_SimpleString(pythonPrefix[5].c_str());
PyRun_SimpleString(pythonPrefix[6].c_str());
PyRun_SimpleString(pythonPrefix[7].c_str());
PyRun_SimpleString("import linecache");
// Allow threads
_pyState = PyEval_SaveThread();
}
Interpreter::~Interpreter() {
PyEval_RestoreThread(_pyState);
// Ideally, we should call Py_Finalize but this is not really supported by
// Python.
// Instead, we merelly remove variables.
// Code was taken here:
// https://github.com/numpy/numpy/issues/8097#issuecomment-356683953
{
PyObject* poAttrList = PyObject_Dir(mainmod_);
PyObject* poAttrIter = PyObject_GetIter(poAttrList);
PyObject* poAttrName;
while ((poAttrName = PyIter_Next(poAttrIter)) != NULL) {
std::string oAttrName(obj_to_str(poAttrName));
// Make sure we don't delete any private objects.
if (oAttrName.compare(0, 2, "__") != 0 ||
oAttrName.compare(oAttrName.size() - 2, 2, "__") != 0) {
PyObject* poAttr = PyObject_GetAttr(mainmod_, poAttrName);
// Make sure we don't delete any module objects.
if (poAttr && poAttr->ob_type != mainmod_->ob_type)
PyObject_SetAttr(mainmod_, poAttrName, NULL);
Py_DECREF(poAttr);
}
Py_DECREF(poAttrName);
}
Py_DECREF(poAttrIter);
Py_DECREF(poAttrList);
}
Py_DECREF(mainmod_);
Py_DECREF(globals_);
// Py_Finalize();
}
std::string Interpreter::python(const std::string& command) {
std::string lerr(""), lout(""), lres("");
python(command, lres, lout, lerr);
return lres;
}
void Interpreter::python(const std::string& command, std::string& res,
std::string& out, std::string& err) {
res = "";
out = "";
err = "";
// Check if the command is not a python comment or empty.
std::string::size_type iFirstNonWhite = command.find_first_not_of(" \t");
// Empty command
if (iFirstNonWhite == std::string::npos) return;
// Command is a comment. Ignore it.
if (command[iFirstNonWhite] == '#') return;
PyEval_RestoreThread(_pyState);
std::cout << command.c_str() << std::endl;
PyObject* result =
PyRun_String(command.c_str(), Py_eval_input, globals_, globals_);
// Check if the result is null.
if (result == NULL) {
// Test if this is a syntax error (due to the evaluation of an expression)
// else just output the problem.
if (!HandleErr(err, globals_, Py_eval_input)) {
// If this is a statement, re-parse the command.
result =
PyRun_String(command.c_str(), Py_single_input, globals_, globals_);
// If there is still an error build the appropriate err string.
if (result == NULL) HandleErr(err, globals_, Py_single_input);
// If there is no error, make sure that the previous error message is
// erased.
else
err = "";
} else
dgDEBUG(15) << "Do not try a second time." << std::endl;
}
PyObject* stdout_obj = 0;
stdout_obj =
PyRun_String("stdout_catcher.fetch()", Py_eval_input, globals_, globals_);
out = obj_to_str(stdout_obj);
Py_DECREF(stdout_obj);
// Local display for the robot (in debug mode or for the logs)
if (out.size() != 0) std::cout << "Output:" << out << std::endl;
if (err.size() != 0) std::cout << "Error:" << err << std::endl;
// If python cannot build a string representation of result
// then results is equal to NULL. This will trigger a SEGV
dgDEBUG(15) << "For command: " << command << std::endl;
if (result != NULL) {
res = obj_to_str(result);
dgDEBUG(15) << "Result is: " << res << std::endl;
Py_DECREF(result);
} else {
dgDEBUG(15) << "Result is: empty" << std::endl;
}
dgDEBUG(15) << "Out is: " << out << std::endl;
dgDEBUG(15) << "Err is :" << err << std::endl;
_pyState = PyEval_SaveThread();
return;
}
PyObject* Interpreter::globals() { return globals_; }
void Interpreter::runPythonFile(std::string filename) {
std::string err = "";
runPythonFile(filename, err);
}
void Interpreter::runPythonFile(std::string filename, std::string& err) {
FILE* pFile = fopen(filename.c_str(), "r");
if (pFile == 0x0) {
err = filename + " cannot be open";
return;
}
PyEval_RestoreThread(_pyState);
err = "";
PyObject* run =
PyRun_File(pFile, filename.c_str(), Py_file_input, globals_, globals_);
if (run == NULL) {
HandleErr(err, globals_, Py_file_input);
std::cerr << err << std::endl;
}
Py_DecRef(run);
_pyState = PyEval_SaveThread();
fclose(pFile);
}
void Interpreter::runMain(void) {
PyEval_RestoreThread(_pyState);
#if PY_MAJOR_VERSION >= 3
const Py_UNICODE* argv[] = {L"dg-embedded-pysh"};
Py_Main(1, const_cast<Py_UNICODE**>(argv));
#else
const char* argv[] = {"dg-embedded-pysh"};
Py_Main(1, const_cast<char**>(argv));
#endif
_pyState = PyEval_SaveThread();
}
std::string Interpreter::processStream(std::istream& stream, std::ostream& os) {
char line[10000];
sprintf(line, "%s", "\n");
std::string command;
std::streamsize maxSize = 10000;
#if 0
while (line != std::string("")) {
stream.getline(line, maxSize, '\n');
command += std::string(line) + std::string("\n");
};
#else
os << "dg> ";
stream.getline(line, maxSize, ';');
command += std::string(line);
#endif
return command;
}
} // namespace python
} // namespace dynamicgraph
/*
* Copyright 2010 (C) CNRS
* Author: Florent Lamiraux
*/
#include <Python.h>
#include <iostream>
#include <sstream>
#include <dynamic-graph/signal-base.h>
#include <dynamic-graph/signal-caster.h>
using dynamicgraph::SignalBase;
namespace dynamicgraph {
namespace python {
extern PyObject* error;
namespace signalBase {
static void destroy (void* self);
/**
\brief Create an instance of SignalBase
*/
PyObject* create(PyObject* self, PyObject* args)
{
char *name = NULL;
if (!PyArg_ParseTuple(args, "s", &name))
return NULL;
SignalBase<int>* obj = NULL;
obj = new SignalBase<int>(std::string(name));
// Return the pointer
return PyCObject_FromVoidPtr((void*)obj, destroy);
}
/**
\brief Destroy an instance of InvertedPendulum
*/
static void destroy (void* self)
{
SignalBase<int>* obj = (SignalBase<int>*)self;
delete obj;
}
PyObject* getTime(PyObject* self, PyObject* args)
{
void* pointer = NULL;
PyObject* object = NULL;
if (!PyArg_ParseTuple(args,"O", &object))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
SignalBase<int>* obj = (SignalBase<int>*)pointer;
int time = obj->getTime();
return Py_BuildValue("i", time);
}
PyObject* getValue(PyObject* self, PyObject* args)
{
void* pointer = NULL;
PyObject* object = NULL;
if (!PyArg_ParseTuple(args,"O", &object))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
SignalBase<int>* signal = (SignalBase<int>*)pointer;
std::ostringstream value;
try {
signal->get(value);
} catch (const dynamicgraph::ExceptionAbstract& exc) {
PyErr_SetString(error, exc.getStringMessage().c_str());
return NULL;
} catch (const std::exception& exc) {
PyErr_SetString(error, exc.what());
} catch (...) {
PyErr_SetString(error, "Unknown exception");
return NULL;
}
std::string valueString = value.str();
return Py_BuildValue("s", valueString.c_str());
}
PyObject* getName(PyObject* self, PyObject* args)
{
void* pointer = NULL;
PyObject* object = NULL;
if (!PyArg_ParseTuple(args,"O", &object))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
SignalBase<int>* signal = (SignalBase<int>*)pointer;
std::string name;
try {
name = signal->getName();
} catch (const dynamicgraph::ExceptionAbstract& exc) {
PyErr_SetString(error, exc.getStringMessage().c_str());
return NULL;
} catch (const std::exception& exc) {
PyErr_SetString(error, exc.what());
} catch (...) {
PyErr_SetString(error, "Unknown exception");
return NULL;
}
return Py_BuildValue("s", name.c_str());
}
PyObject* setValue(PyObject* self, PyObject* args)
{
void * pointer = NULL;
PyObject* object = NULL;
char* valueString = NULL;
if (!PyArg_ParseTuple(args,"Os", &object, &valueString))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
SignalBase<int>* signal = (SignalBase<int>*)pointer;
std::ostringstream os;
os << valueString;
std::istringstream value(os.str());
try {
signal->set(value);
} catch (const dynamicgraph::ExceptionAbstract& exc) {
PyErr_SetString(error, exc.getStringMessage().c_str());
return NULL;
} catch (const std::exception& exc) {
PyErr_SetString(error, exc.what());
return NULL;
} catch (...) {
PyErr_SetString(error, "Unknown exception");
return NULL;
}
return Py_BuildValue("");
}
PyObject* recompute(PyObject* self, PyObject* args)
{
void * pointer = NULL;
PyObject* object = NULL;
unsigned int time;
if (!PyArg_ParseTuple(args,"OI", &object, &time))
return NULL;
if (!PyCObject_Check(object))
return NULL;
pointer = PyCObject_AsVoidPtr(object);
SignalBase<int>* signal = (SignalBase<int>*)pointer;
try {
signal->recompute(time);
} catch (const std::exception& exc) {
PyErr_SetString(error, exc.what());
}
return Py_BuildValue("");
}
}
}
}
/*
* Copyright 2010 (C) CNRS
* Author: Florent Lamiraux
*/
#include <Python.h>
#include <iostream>
#include <sstream>
#include <dynamic-graph/signal-caster.h>
namespace dynamicgraph {
namespace python {
extern PyObject* error;
namespace signalCaster {
PyObject* getSignalTypeList(PyObject* self, PyObject* args)
{
void * pointer = NULL;
PyObject* object = NULL;
if (!PyArg_ParseTuple(args, ""))
return NULL;
std::vector<std::string> typeList =
dynamicgraph::g_caster.listTypenames();
unsigned int typeNumber = typeList.size();
// Build a tuple object
PyObject* typeTuple = PyTuple_New(typeNumber);
for (unsigned int iType = 0; iType < typeNumber; iType++) {
PyObject* className = Py_BuildValue("s", typeList[iType].c_str());
PyTuple_SetItem(typeTuple, iType, className);
}
return Py_BuildValue("O", typeTuple);
}
} //namespace signalCaster
} // namespace dynamicgraph
} // namespace python
# Copyright 2010-2020, Florent Lamiraux, Thomas Moulard, Olivier Stasse, Guilhem
# Saurel, JRL, CNRS/AIST, LAAS-CNRS
# Test the interpreter
add_unit_test(interpreter-test interpreter-test.cc)
target_link_libraries(interpreter-test PRIVATE ${PROJECT_NAME})
# Test runfile
add_unit_test(interpreter-test-runfile interpreter-test-runfile.cc)
target_link_libraries(interpreter-test-runfile PRIVATE ${PROJECT_NAME})
target_include_directories(interpreter-test-runfile
PRIVATE Boost::unit_test_framework)
target_compile_definitions(interpreter-test-runfile
PRIVATE PATH="${CMAKE_CURRENT_LIST_DIR}/")
# Test the module generation Create an entity
set(LIBRARY_NAME "custom_entity")
add_library(${LIBRARY_NAME} SHARED "${LIBRARY_NAME}.cpp")
if(SUFFIX_SO_VERSION)
set_target_properties(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
endif(SUFFIX_SO_VERSION)
target_link_libraries(${LIBRARY_NAME} PRIVATE dynamic-graph::dynamic-graph)
# Create its bindings This mimics DYNAMIC_GRAPH_PYTHON_MODULE(${LIBRARY_NAME}
# ${LIBRARY_NAME} "${LIBRARY_NAME}-wrap")
configure_file(
${PROJECT_SOURCE_DIR}/cmake/dynamic_graph/submodule/__init__.py.cmake
${CMAKE_CURRENT_BINARY_DIR}/${LIBRARY_NAME}/__init__.py)
set(PYTHON_MODULE "${LIBRARY_NAME}-wrap")
set(DYNAMICGRAPH_MODULE_HEADER
"${CMAKE_SOURCE_DIR}/tests/custom_entity_module.h")
configure_file(${PROJECT_SOURCE_DIR}/cmake/dynamic_graph/python-module-py.cc.in
${CMAKE_CURRENT_BINARY_DIR}/python-module-py.cc @ONLY)
add_library(${PYTHON_MODULE} MODULE
${CMAKE_CURRENT_BINARY_DIR}/python-module-py.cc)
set_target_properties(${PYTHON_MODULE}
PROPERTIES PREFIX "" OUTPUT_NAME ${LIBRARY_NAME}/wrap)
if(UNIX AND NOT APPLE)
target_link_libraries(${PYTHON_MODULE} PRIVATE "-Wl,--no-as-needed")
endif(UNIX AND NOT APPLE)
target_link_libraries(${PYTHON_MODULE} PRIVATE ${LIBRARY_NAME} ${PROJECT_NAME})
# Test it
add_python_unit_test("test-custom-entity" "tests/test_custom_entity.py" src
tests)
# also test other bindings, using this custom entity
add_python_unit_test("test-bindings" "tests/test_bindings.py" src tests)