Skip to content

Commit

Permalink
Merge pull request #325 from ManifoldFR/wj/move-std-vector
Browse files Browse the repository at this point in the history
Copy std-vector and std-map from Pinocchio
  • Loading branch information
jcarpent authored Dec 5, 2022
2 parents 15fb426 + be7c2ef commit 5ae3793
Show file tree
Hide file tree
Showing 17 changed files with 877 additions and 71 deletions.
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ repos:
rev: v15.0.4
hooks:
- id: clang-format
args: [--style=Google]
args: ['--style={BasedOnStyle: Google, SortIncludes: false}']
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.4.0
hooks:
Expand Down
5 changes: 5 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,7 @@ set(${PROJECT_NAME}_HEADERS
include/eigenpy/exception.hpp
include/eigenpy/scalar-conversion.hpp
include/eigenpy/expose.hpp
include/eigenpy/copyable.hpp
include/eigenpy/details.hpp
include/eigenpy/fwd.hpp
include/eigenpy/eigen-allocator.hpp
Expand All @@ -154,6 +155,9 @@ set(${PROJECT_NAME}_HEADERS
include/eigenpy/user-type.hpp
include/eigenpy/ufunc.hpp
include/eigenpy/register.hpp
include/eigenpy/std-map.hpp
include/eigenpy/std-vector.hpp
include/eigenpy/pickle-vector.hpp
include/eigenpy/stride.hpp
include/eigenpy/swig.hpp
include/eigenpy/version.hpp)
Expand Down Expand Up @@ -194,6 +198,7 @@ set(${PROJECT_NAME}_SOURCES
src/angle-axis.cpp
src/quaternion.cpp
src/geometry-conversion.cpp
src/std-vector.cpp
src/version.cpp)

add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}
Expand Down
2 changes: 1 addition & 1 deletion cmake
30 changes: 30 additions & 0 deletions include/eigenpy/copyable.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
//
// Copyright (c) 2016-2021 CNRS INRIA
//

#ifndef __eigenpy_utils_copyable_hpp__
#define __eigenpy_utils_copyable_hpp__

#include <boost/python.hpp>

namespace eigenpy {

namespace bp = boost::python;

///
/// \brief Add the Python method copy to allow a copy of this by calling the
/// copy constructor.
///
template <class C>
struct CopyableVisitor : public bp::def_visitor<CopyableVisitor<C> > {
template <class PyClass>
void visit(PyClass& cl) const {
cl.def("copy", &copy, bp::arg("self"), "Returns a copy of *this.");
}

private:
static C copy(const C& self) { return C(self); }
};
} // namespace eigenpy

#endif // ifndef __eigenpy_utils_copyable_hpp__
42 changes: 22 additions & 20 deletions include/eigenpy/eigen-allocator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -232,6 +232,22 @@ struct EigenAllocator {
};

#if EIGEN_VERSION_AT_LEAST(3, 2, 0)
/// @brief Check if we need to allocate @tparam MatType to convert @param
/// pyArray.
/// @details do not allocate if:
/// want row-major & data C-contiguous OR
/// want col-major & data F-contiguous OR
/// you want a compile-time vector
/// in these cases, data layout fits desired view layout
template <typename MatType>
inline bool is_arr_layout_compatible_with_mat_type(PyArrayObject *pyArray) {
bool is_array_C_cont = PyArray_IS_C_CONTIGUOUS(pyArray);
bool is_array_F_cont = PyArray_IS_F_CONTIGUOUS(pyArray);
return (MatType::IsRowMajor && is_array_C_cont) ||
(!MatType::IsRowMajor && is_array_F_cont) ||
MatType::IsVectorAtCompileTime;
}

template <typename MatType, int Options, typename Stride>
struct EigenAllocator<Eigen::Ref<MatType, Options, Stride> > {
typedef Eigen::Ref<MatType, Options, Stride> RefType;
Expand All @@ -255,16 +271,9 @@ struct EigenAllocator<Eigen::Ref<MatType, Options, Stride> > {
const int pyArray_type_code = EIGENPY_GET_PY_ARRAY_TYPE(pyArray);
const int Scalar_type_code = Register::getTypeCode<Scalar>();
if (pyArray_type_code != Scalar_type_code) need_to_allocate |= true;
if ((MatType::IsRowMajor && (PyArray_IS_C_CONTIGUOUS(pyArray) &&
!PyArray_IS_F_CONTIGUOUS(pyArray))) ||
(!MatType::IsRowMajor && (PyArray_IS_F_CONTIGUOUS(pyArray) &&
!PyArray_IS_C_CONTIGUOUS(pyArray))) ||
MatType::IsVectorAtCompileTime ||
(PyArray_IS_F_CONTIGUOUS(pyArray) &&
PyArray_IS_C_CONTIGUOUS(pyArray))) // no need to allocate
need_to_allocate |= false;
else
need_to_allocate |= true;
bool incompatible_layout =
!is_arr_layout_compatible_with_mat_type<MatType>(pyArray);
need_to_allocate |= incompatible_layout;
if (Options !=
Eigen::Unaligned) // we need to check whether the memory is correctly
// aligned and composed of a continuous segment
Expand Down Expand Up @@ -365,16 +374,9 @@ struct EigenAllocator<const Eigen::Ref<const MatType, Options, Stride> > {
const int Scalar_type_code = Register::getTypeCode<Scalar>();

if (pyArray_type_code != Scalar_type_code) need_to_allocate |= true;
if ((MatType::IsRowMajor && (PyArray_IS_C_CONTIGUOUS(pyArray) &&
!PyArray_IS_F_CONTIGUOUS(pyArray))) ||
(!MatType::IsRowMajor && (PyArray_IS_F_CONTIGUOUS(pyArray) &&
!PyArray_IS_C_CONTIGUOUS(pyArray))) ||
MatType::IsVectorAtCompileTime ||
(PyArray_IS_F_CONTIGUOUS(pyArray) &&
PyArray_IS_C_CONTIGUOUS(pyArray))) // no need to allocate
need_to_allocate |= false;
else
need_to_allocate |= true;
bool incompatible_layout =
!is_arr_layout_compatible_with_mat_type<MatType>(pyArray);
need_to_allocate |= incompatible_layout;
if (Options !=
Eigen::Unaligned) // we need to check whether the memory is correctly
// aligned and composed of a continuous segment
Expand Down
46 changes: 46 additions & 0 deletions include/eigenpy/pickle-vector.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
//
// Copyright (c) 2019-2020 CNRS INRIA
//

#ifndef __eigenpy_utils_pickle_vector_hpp__
#define __eigenpy_utils_pickle_vector_hpp__

#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/tuple.hpp>

namespace eigenpy {
///
/// \brief Create a pickle interface for the std::vector
///
/// \tparam VecType Vector Type to pickle
///
template <typename VecType>
struct PickleVector : boost::python::pickle_suite {
static boost::python::tuple getinitargs(const VecType&) {
return boost::python::make_tuple();
}

static boost::python::tuple getstate(boost::python::object op) {
return boost::python::make_tuple(
boost::python::list(boost::python::extract<const VecType&>(op)()));
}

static void setstate(boost::python::object op, boost::python::tuple tup) {
if (boost::python::len(tup) > 0) {
VecType& o = boost::python::extract<VecType&>(op)();
boost::python::stl_input_iterator<typename VecType::value_type> begin(
tup[0]),
end;
while (begin != end) {
o.push_back(*begin);
++begin;
}
}
}

static bool getstate_manages_dict() { return true; }
};
} // namespace eigenpy

#endif // ifndef __eigenpy_utils_pickle_vector_hpp__
8 changes: 4 additions & 4 deletions include/eigenpy/quaternion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -294,8 +294,8 @@ class QuaternionVisitor
return q;
}

static Quaternion* FromTwoVectors(const Eigen::Ref<Vector3> u,
const Eigen::Ref<Vector3> v) {
static Quaternion* FromTwoVectors(const Eigen::Ref<const Vector3> u,
const Eigen::Ref<const Vector3> v) {
Quaternion* q(new Quaternion);
q->setFromTwoVectors(u, v);
return q;
Expand All @@ -308,12 +308,12 @@ class QuaternionVisitor

static Quaternion* DefaultConstructor() { return new Quaternion; }

static Quaternion* FromOneVector(const Eigen::Ref<Vector4> v) {
static Quaternion* FromOneVector(const Eigen::Ref<const Vector4> v) {
Quaternion* q(new Quaternion(v[3], v[0], v[1], v[2]));
return q;
}

static Quaternion* FromRotationMatrix(const Eigen::Ref<Matrix3> R) {
static Quaternion* FromRotationMatrix(const Eigen::Ref<const Matrix3> R) {
Quaternion* q(new Quaternion(R));
return q;
}
Expand Down
64 changes: 64 additions & 0 deletions include/eigenpy/std-map.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
/// Copyright (c) 2016-2022 CNRS INRIA
/// This file was taken from Pinocchio (header
/// <pinocchio/bindings/python/utils/std-vector.hpp>)
///

#ifndef __eigenpy_utils_map_hpp__
#define __eigenpy_utils_map_hpp__

#include <boost/python/suite/indexing/map_indexing_suite.hpp>

namespace eigenpy {
namespace details {
template <typename Container>
struct overload_base_get_item_for_std_map
: public boost::python::def_visitor<
overload_base_get_item_for_std_map<Container> > {
typedef typename Container::value_type value_type;
typedef typename Container::value_type::second_type data_type;
typedef typename Container::key_type key_type;
typedef typename Container::key_type index_type;

template <class Class>
void visit(Class& cl) const {
cl.def("__getitem__", &base_get_item);
}

private:
static boost::python::object base_get_item(
boost::python::back_reference<Container&> container, PyObject* i_) {
namespace bp = ::boost::python;

index_type idx = convert_index(container.get(), i_);
typename Container::iterator i = container.get().find(idx);
if (i == container.get().end()) {
PyErr_SetString(PyExc_KeyError, "Invalid key");
bp::throw_error_already_set();
}

typename bp::to_python_indirect<data_type&,
bp::detail::make_reference_holder>
convert;
return bp::object(bp::handle<>(convert(i->second)));
}

static index_type convert_index(Container& /*container*/, PyObject* i_) {
namespace bp = ::boost::python;
bp::extract<key_type const&> i(i_);
if (i.check()) {
return i();
} else {
bp::extract<key_type> i(i_);
if (i.check()) return i();
}

PyErr_SetString(PyExc_TypeError, "Invalid index type");
bp::throw_error_already_set();
return index_type();
}
};

} // namespace details
} // namespace eigenpy

#endif // ifndef __eigenpy_utils_map_hpp__
Loading

0 comments on commit 5ae3793

Please sign in to comment.