Commit 84350b35 authored by Christian Mandery's avatar Christian Mandery

Merge branch 'api-changes' into 'master'

Expose MotionReaderXML, Motion and MotionFrame. Add Eigen and Numpy libraries

See merge request !1
parents 415b3d03 36ec00d7
/*
* The Biomechanical ToolKit
* Copyright (c) 2009-2013, Arnaud Barré
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above
* copyright notice, this list of conditions and the following
* disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials
* provided with the distribution.
* * Neither the name(s) of the copyright holders nor the names
* of its contributors may be used to endorse or promote products
* derived from this software without specific prior written
* permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
%{
#define SWIG_FILE_WITH_INIT
#include "Eigen/Core"
%}
%include "numpy.i"
%init
%{
import_array();
%}
%fragment("Eigen_Fragments", "header", fragment="NumPy_Fragments")
%{
template <typename T> int NumPyType() {return -1;};
template <class Derived>
void ConvertFromNumpyToEigenMatrix(Eigen::MatrixBase<Derived>* out, PyObject* in)
{
int rows = 0;
int cols = 0;
// Check object type
if (!is_array(in))
{
PyErr_SetString(PyExc_ValueError, "The given input is not known as a NumPy array or matrix.");
return;
}
// Check data type
else if (array_type(in) != NumPyType<typename Derived::Scalar>())
{
PyErr_SetString(PyExc_ValueError, "Type mismatch between NumPy and Eigen objects.");
return;
}
// Check dimensions
else if (array_numdims(in) > 2)
{
PyErr_SetString(PyExc_ValueError, "Eigen only support 1D or 2D array.");
return;
}
else if (array_numdims(in) == 1)
{
rows = array_size(in,0);
cols = 1;
if ((Derived::RowsAtCompileTime != Eigen::Dynamic) && (Derived::RowsAtCompileTime != rows))
{
PyErr_SetString(PyExc_ValueError, "Row dimension mismatch between NumPy and Eigen objects (1D).");
return;
}
else if ((Derived::ColsAtCompileTime != Eigen::Dynamic) && (Derived::ColsAtCompileTime != 1))
{
PyErr_SetString(PyExc_ValueError, "Column dimension mismatch between NumPy and Eigen objects (1D).");
return;
}
}
else if (array_numdims(in) == 2)
{
rows = array_size(in,0);
cols = array_size(in,1);
if ((Derived::RowsAtCompileTime != Eigen::Dynamic) && (Derived::RowsAtCompileTime != array_size(in,0)))
{
PyErr_SetString(PyExc_ValueError, "Row dimension mismatch between NumPy and Eigen objects (2D).");
return;
}
else if ((Derived::ColsAtCompileTime != Eigen::Dynamic) && (Derived::ColsAtCompileTime != array_size(in,1)))
{
PyErr_SetString(PyExc_ValueError, "Column dimension mismatch between NumPy and Eigen objects (2D).");
return;
}
}
// Extract data
int isNewObject = 0;
PyArrayObject* temp = obj_to_array_contiguous_allow_conversion(in, array_type(in), &isNewObject);
if (temp == NULL)
{
PyErr_SetString(PyExc_ValueError, "Impossible to convert the input into a Python array object.");
return;
}
out->derived().setZero(rows, cols);
typename Derived::Scalar* data = static_cast<typename Derived::Scalar*>(PyArray_DATA(temp));
for (int i = 0; i != rows; ++i)
for (int j = 0; j != cols; ++j)
out->coeffRef(i,j) = data[i*cols+j];
};
// Copies values from Eigen type into an existing NumPy type
template <class Derived>
void CopyFromEigenToNumPyMatrix(PyObject* out, Eigen::MatrixBase<Derived>* in)
{
int rows = 0;
int cols = 0;
// Check object type
if (!is_array(out))
{
PyErr_SetString(PyExc_ValueError, "The given input is not known as a NumPy array or matrix.");
return;
}
// Check data type
else if (array_type(out) != NumPyType<typename Derived::Scalar>())
{
PyErr_SetString(PyExc_ValueError, "Type mismatch between NumPy and Eigen objects.");
return;
}
// Check dimensions
else if (array_numdims(out) > 2)
{
PyErr_SetString(PyExc_ValueError, "Eigen only support 1D or 2D array.");
return;
}
else if (array_numdims(out) == 1)
{
rows = array_size(out,0);
cols = 1;
if ((Derived::RowsAtCompileTime != Eigen::Dynamic) && (Derived::RowsAtCompileTime != rows))
{
PyErr_SetString(PyExc_ValueError, "Row dimension mismatch between NumPy and Eigen objects (1D).");
return;
}
else if ((Derived::ColsAtCompileTime != Eigen::Dynamic) && (Derived::ColsAtCompileTime != 1))
{
PyErr_SetString(PyExc_ValueError, "Column dimension mismatch between NumPy and Eigen objects (1D).");
return;
}
}
else if (array_numdims(out) == 2)
{
rows = array_size(out,0);
cols = array_size(out,1);
}
if (in->cols() != cols || in->rows() != rows) {
/// TODO: be forgiving and simply create or resize the array
PyErr_SetString(PyExc_ValueError, "Dimension mismatch between NumPy and Eigen object (return argument).");
return;
}
// Extract data
int isNewObject = 0;
PyArrayObject* temp = obj_to_array_contiguous_allow_conversion(out, array_type(out), &isNewObject);
if (temp == NULL)
{
PyErr_SetString(PyExc_ValueError, "Impossible to convert the input into a Python array object.");
return;
}
typename Derived::Scalar* data = static_cast<typename Derived::Scalar*>(PyArray_DATA(out));
for (int i = 0; i != in->rows(); ++i) {
for (int j = 0; j != in->cols(); ++j) {
data[i*in->cols()+j] = in->coeff(i,j);
}
}
};
template <class Derived>
void ConvertFromEigenToNumPyMatrix(PyObject** out, Eigen::MatrixBase<Derived>* in)
{
npy_intp dims[2] = {in->rows(), in->cols()};
*out = PyArray_SimpleNew(2, dims, NumPyType<typename Derived::Scalar>());
typename Derived::Scalar* data = static_cast<typename Derived::Scalar*>(PyArray_DATA(*out));
for (int i = 0; i != dims[0]; ++i)
for (int j = 0; j != dims[1]; ++j)
data[i*dims[1]+j] = in->coeff(i,j);
};
template<> int NumPyType<float>() {return PyArray_FLOAT;};
template<> int NumPyType<double>() {return PyArray_DOUBLE;};
%}
// ----------------------------------------------------------------------------
// Macro to create the typemap for Eigen classes
// ----------------------------------------------------------------------------
%define %eigen_typemaps(CLASS)
// Argout: const & (Disabled and prevents calling of the non-const typemap)
%typemap(argout, fragment="Eigen_Fragments") const CLASS & ""
// Argout: & (for returning values to in-out arguments)
%typemap(argout, fragment="Eigen_Fragments") CLASS &
{
// Argout: &
CopyFromEigenToNumPyMatrix<CLASS>($input, $1);
}
// In: (nothing: no constness)
%typemap(in, fragment="Eigen_Fragments") CLASS (CLASS temp)
{
ConvertFromNumpyToEigenMatrix<CLASS>(&temp, $input);
$1 = temp;
}
// In: const&
%typemap(in, fragment="Eigen_Fragments") CLASS const& (CLASS temp)
{
// In: const&
ConvertFromNumpyToEigenMatrix<CLASS>(&temp, $input);
$1 = &temp;
}
// In: & (not yet implemented)
%typemap(in, fragment="Eigen_Fragments") CLASS & (CLASS temp)
{
// In: non-const&
ConvertFromNumpyToEigenMatrix<CLASS>(&temp, $input);
$1 = &temp;
}
// In: const* (not yet implemented)
%typemap(in, fragment="Eigen_Fragments") CLASS const*
{
PyErr_SetString(PyExc_ValueError, "The input typemap for const pointer is not yet implemented. Please report this problem to the developer.");
}
// In: * (not yet implemented)
%typemap(in, fragment="Eigen_Fragments") CLASS *
{
PyErr_SetString(PyExc_ValueError, "The input typemap for non-const pointer is not yet implemented. Please report this problem to the developer.");
}
// Out: (nothing: no constness)
%typemap(out, fragment="Eigen_Fragments") CLASS
{
ConvertFromEigenToNumPyMatrix<CLASS>(&$result, &$1);
}
// Out: const
%typemap(out, fragment="Eigen_Fragments") CLASS const
{
ConvertFromEigenToNumPyMatrix<CLASS>(&$result, &$1);
}
// Out: const&
%typemap(out, fragment="Eigen_Fragments") CLASS const&
{
ConvertFromEigenToNumPyMatrix<CLASS>(&$result, $1);
}
// Out: & (not yet implemented)
%typemap(out, fragment="Eigen_Fragments") CLASS &
{
PyErr_SetString(PyExc_ValueError, "The output typemap for non-const reference is not yet implemented. Please report this problem to the developer.");
}
// Out: const* (not yet implemented)
%typemap(out, fragment="Eigen_Fragments") CLASS const*
{
PyErr_SetString(PyExc_ValueError, "The output typemap for const pointer is not yet implemented. Please report this problem to the developer.");
}
// Out: * (not yet implemented)
%typemap(out, fragment="Eigen_Fragments") CLASS *
{
PyErr_SetString(PyExc_ValueError, "The output typemap for non-const pointer is not yet implemented. Please report this problem to the developer.");
}
%enddef
This diff is collapsed.
%module pymmm
%{
#include "MMM/Model/Model.h"
#include "MMM/Model/ModelProcessor.h"
#include "MMM/Model/ModelProcessorWinter.h"
#include "MMM/Model/ModelReaderXML.h"
#include "MMM/Motion/MotionReaderXML.h"
#include "MMM/Motion/Motion.h"
#include "MMM/Motion/MotionFrame.h"
#include "MMMSimoxTools/MMMSimoxTools.h"
%}
%include "std_string.i"
%include "std_vector.i"
namespace std {
%template(StdVectorFloat) vector<float>;
%template(StdVectorString) vector<std::string>;
}
%include "boost_shared_ptr.i"
%shared_ptr(MMM::Motion)
%shared_ptr(MMM::MotionFrame)
%shared_ptr(MMM::MotionFrameEntry)
%shared_ptr(MMM::Model)
%shared_ptr(MMM::ModelProcessor)
%template(MotionPtr) boost::shared_ptr<MMM::Motion>;
%template(MotionFramePtr) boost::shared_ptr<MMM::MotionFrame>;
%template(MotionFrameEntryPtr) boost::shared_ptr<MMM::MotionFrameEntry>;
%template(ModelPtr) boost::shared_ptr<MMM::Model>;
%template(ModelProcessorPtr) boost::shared_ptr<MMM::ModelProcessor>;
%include <eigen.i>
%eigen_typemaps(Eigen::Vector3f)
%eigen_typemaps(Eigen::Matrix4f)
// Rename all methods called "print" to "print_instance" because print is a reserved keyword in Python
%rename(print_instance) print;
namespace VirtualRobot {
class Robot;
......@@ -14,9 +46,102 @@ namespace VirtualRobot {
}
namespace MMM {
class Motion;
typedef boost::shared_ptr<Motion> MotionPtr;
class MotionFrame;
typedef boost::shared_ptr<MotionFrame> MotionFramePtr;
class MotionFrameEntry;
typedef boost::shared_ptr<MotionFrameEntry> MotionFrameEntryPtr;
class Model;
typedef boost::shared_ptr<Model> ModelPtr;
class ModelProcessor;
typedef boost::shared_ptr<ModelProcessor> ModelProcessorPtr;
typedef std::vector<MotionPtr> MotionList;
class MotionReaderXML {
public:
MotionReaderXML();
MotionPtr loadMotion(const std::string &xmlFile, const std::string &motionName = std::string());
MotionList loadAllMotions(const std::string &xmlFile);
MotionList loadAllMotionsFromString(const std::string &xmlDataString);
std::vector<std::string> getMotionNames(const std::string &xmlFile) const;
};
typedef boost::shared_ptr<MotionReaderXML> MotionReaderXMLPtr;
class Motion {
public:
Motion(const std::string& name);
Motion(const Motion& m);
bool addMotionFrame(MotionFramePtr md);
bool removeMotionFrame(size_t frame);
bool setJointOrder(const std::vector<std::string> &jointNames);
void setName(const std::string& name);
std::string getName();
void setComment(const std::string &comment);
void addComment(const std::string &comment);
std::string getComment();
std::vector<std::string> getJointNames();
MotionFramePtr getMotionFrame(size_t frame);
std::vector<MotionFramePtr> getMotionFrames();
void setMotionFrames(std::vector<MotionFramePtr> &mfs){motionFrames = mfs;}
virtual unsigned int getNumFrames();
void setModel(ModelPtr model);
void setModel(ModelPtr processedModel, ModelPtr originalModel);
ModelPtr getModel(bool processedModel = true);
const std::string& getMotionFilePath();
void setMotionFilePath(const std::string&filepath);
const std::string& getMotionFileName();
void setMotionFileName(const std::string&filename);
void setModelProcessor(ModelProcessorPtr mp);
ModelProcessorPtr getModelProcessor();
virtual std::string toXML();
void print();
void print(const std::string &filename);
bool hasJoint( const std::string &name );
void calculateVelocities(int method = 0);
void calculateAccelerations(int method = 0);
enum jointEnum{eValues, eVelocities, eAccelerations};
void smoothJointValues(jointEnum type, int windowSize);
MotionPtr getSegmentMotion(size_t frame1, size_t frame2);
MotionPtr copy();
MotionList getSegmentMotions(std::vector<int> segmentPoints);
};
class MotionFrame {
public:
MotionFrame(unsigned int ndof);
MotionFrame(const MotionFrame &inpSrc);
Eigen::Matrix4f getRootPose();
Eigen::Vector3f getRootPos();
Eigen::Vector3f getRootPosVel();
Eigen::Vector3f getRootPosAcc();
Eigen::Vector3f getRootRot();
Eigen::Vector3f getRootRotVel();
Eigen::Vector3f getRootRotAcc();
bool setRootPose(const Eigen::Matrix4f &pose);
bool setRootPos(const Eigen::Vector3f &pos);
bool setRootPosVel(const Eigen::Vector3f &posVel);
bool setRootPosAcc(const Eigen::Vector3f &posAcc);
bool setRootRot(const Eigen::Vector3f &rot);
bool setRootRotVel(const Eigen::Vector3f &rotVel);
bool setRootRotAcc(const Eigen::Vector3f &rotAcc);
float timestep;
Eigen::VectorXf joint;
Eigen::VectorXf joint_vel;
Eigen::VectorXf joint_acc;
bool addEntry(const std::string &name, MotionFrameEntryPtr entry );
bool removeEntry(const std::string &name);
bool hasEntry(const std::string &name) const;
MotionFrameEntryPtr getEntry(const std::string &name);
std::string toXML();
unsigned int getndof();
};
class ModelReaderXML {
public:
ModelReaderXML();
......
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