Commit a9848e8f authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'master' into devel

parents 3df9bea0 0ca806bc
Pipeline #12037 failed with stage
in 10 minutes and 41 seconds
...@@ -7,8 +7,7 @@ after_success: ...@@ -7,8 +7,7 @@ after_success:
- cd _travis/build/doc && ../../../cmake/github/update-doxygen-doc.sh - cd _travis/build/doc && ../../../cmake/github/update-doxygen-doc.sh
branches: branches:
only: only:
- master - disabled_travis_because_not_relevant_anymore
- topic/sot-pinocchio
compiler: compiler:
- clang - clang
- gcc - gcc
......
...@@ -36,18 +36,18 @@ PROJECT(${PROJECT_NAME} ${PROJECT_ARGS}) ...@@ -36,18 +36,18 @@ PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
ADD_PROJECT_DEPENDENCY(sot-tools REQUIRED PKG_CONFIG_REQUIRES sot-tools) ADD_PROJECT_DEPENDENCY(sot-tools REQUIRED PKG_CONFIG_REQUIRES sot-tools)
ADD_PROJECT_DEPENDENCY(example-robot-data) ADD_PROJECT_DEPENDENCY(example-robot-data)
SET(BOOST_COMPONENTS filesystem system unit_test_framework) IF(BUILD_TESTING)
FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
ENDIF(BUILD_TESTING)
IF(BUILD_PYTHON_INTERFACE) IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON() FINDPYTHON()
SEARCH_FOR_BOOST_PYTHON(REQUIRED)
STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR}) STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR})
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED
PKG_CONFIG_REQUIRES dynamic-graph-python) PKG_CONFIG_REQUIRES dynamic-graph-python)
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
ENDIF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_PYTHON_INTERFACE)
SEARCH_FOR_BOOST()
# Main Library # Main Library
SET(${PROJECT_NAME}_HEADERS SET(${PROJECT_NAME}_HEADERS
include/${CUSTOM_HEADER_DIR}/dynamic-pinocchio.h include/${CUSTOM_HEADER_DIR}/dynamic-pinocchio.h
...@@ -69,8 +69,7 @@ SET(${PROJECT_NAME}_SOURCES ...@@ -69,8 +69,7 @@ SET(${PROJECT_NAME}_SOURCES
ADD_LIBRARY(${PROJECT_NAME} SHARED ADD_LIBRARY(${PROJECT_NAME} SHARED
${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>) TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES} TARGET_LINK_LIBRARIES(${PROJECT_NAME} sot-core::sot-core)
sot-core::sot-core)
IF(SUFFIX_SO_VERSION) IF(SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION}) SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
......
Subproject commit be594b8bf3a9003ae17113771655ad5ce8363fa0 Subproject commit 9dcde03a880cccc86531019a6845708f5c54c35d
...@@ -17,6 +17,7 @@ ...@@ -17,6 +17,7 @@
/* STD */ /* STD */
#include <string> #include <string>
#include <map> #include <map>
#include <memory>
/* SOT */ /* SOT */
#include <pinocchio/fwd.hpp> #include <pinocchio/fwd.hpp>
...@@ -29,6 +30,7 @@ ...@@ -29,6 +30,7 @@
#include <sot/core/matrix-geometry.hh> #include <sot/core/matrix-geometry.hh>
/* Matrix */ /* Matrix */
#include <dynamic-graph/linear-algebra.h> #include <dynamic-graph/linear-algebra.h>
#include <sot/dynamic-pinocchio/deprecated.hh>
/* PINOCCHIO */ /* PINOCCHIO */
#include <pinocchio/macros.hpp> #include <pinocchio/macros.hpp>
...@@ -80,7 +82,7 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity { ...@@ -80,7 +82,7 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
/* --- MODEL ATRIBUTES --- */ /* --- MODEL ATRIBUTES --- */
pinocchio::Model* m_model; pinocchio::Model* m_model;
pinocchio::Data* m_data; std::unique_ptr<pinocchio::Data> m_data;
/* --- MODEL ATRIBUTES --- */ /* --- MODEL ATRIBUTES --- */
...@@ -164,7 +166,15 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity { ...@@ -164,7 +166,15 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
void setModel(pinocchio::Model*); void setModel(pinocchio::Model*);
void setData(pinocchio::Data*); void createData();
/// \deprecated this function does nothing. This class has its own
/// pinocchio::Data object, which can be access with \ref getData.
void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;
pinocchio::Model* getModel() { return m_model; };
pinocchio::Data* getData() { return m_data.get(); };
/* --- GETTERS --- */ /* --- GETTERS --- */
......
<package format="2"> <package format="2">
<name>sot-dynamic-pinocchio</name> <name>sot-dynamic-pinocchio</name>
<version>3.5.2</version> <version>3.6.0</version>
<description> <description>
Pinocchio bindings for dynamic-graph Pinocchio bindings for dynamic-graph
</description> </description>
......
...@@ -39,10 +39,15 @@ FOREACH(plugin ${plugins}) ...@@ -39,10 +39,15 @@ FOREACH(plugin ${plugins})
IF(BUILD_PYTHON_INTERFACE) IF(BUILD_PYTHON_INTERFACE)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME}) STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
DYNAMIC_GRAPH_PYTHON_MODULE("${PYTHON_DIR}/${PYTHON_LIBRARY_NAME}" if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap DYNAMIC_GRAPH_PYTHON_MODULE("${PYTHON_DIR}/${PYTHON_LIBRARY_NAME}"
1 "src/python-module-py.cpp") ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
TARGET_LINK_BOOST_PYTHON(${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap) SOURCE_PYTHON_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
DYNAMIC_GRAPH_PYTHON_MODULE("${PYTHON_DIR}/${PYTHON_LIBRARY_NAME}"
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
endif()
ENDIF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_PYTHON_INTERFACE)
ENDFOREACH(plugin) ENDFOREACH(plugin)
......
#include <sot/dynamic-pinocchio/angle-estimator.h>
typedef boost::mpl::vector< dynamicgraph::sot::AngleEstimator > entities_t;
...@@ -75,8 +75,7 @@ class GetJointNames : public Command { ...@@ -75,8 +75,7 @@ class GetJointNames : public Command {
} }
const std::vector<std::string>& jointNames = robot.m_model->names; const std::vector<std::string>& jointNames = robot.m_model->names;
// Remove first joint names 'universe' // Remove first joint names 'universe'
std::size_t n(jointNames.size()); assert(jointNames.size() >= 1);
assert(n >= 1);
std::vector<Value> res; std::vector<Value> res;
for (std::size_t i = 1; i < jointNames.size(); ++i) { for (std::size_t i = 1; i < jointNames.size(); ++i) {
res.push_back(Value(jointNames[i])); res.push_back(Value(jointNames[i]));
......
#include <dynamic-graph/python/module.hh>
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
namespace dg = dynamicgraph;
namespace dgs = dynamicgraph::sot;
typedef bp::return_value_policy<bp::reference_existing_object> reference_existing_object;
BOOST_PYTHON_MODULE(wrap)
{
bp::import("dynamic_graph");
bp::import("pinocchio");
dg::python::exposeEntity<dgs::DynamicPinocchio, bp::bases<dg::Entity>, dg::python::AddCommands>()
.add_property("model",
bp::make_function(&dgs::DynamicPinocchio::getModel, reference_existing_object()),
bp::make_function(&dgs::DynamicPinocchio::setModel))
.add_property("data",
bp::make_function(&dgs::DynamicPinocchio::getData, reference_existing_object()),
bp::make_function(&dgs::DynamicPinocchio::setData))
.def("setModel", &dgs::DynamicPinocchio::setModel)
.def("createData", &dgs::DynamicPinocchio::createData)
.def("setData", &dgs::DynamicPinocchio::setData)
;
}
import numpy as np import numpy as np
from numpy import cos, sin, sqrt from numpy import cos, sin, sqrt
from .dynamic import DynamicPinocchio as DynamicCpp from .dynamic import DynamicPinocchio
# DynamicOld = Dynamic
class DynamicPinocchio(DynamicCpp):
def __init__(self, name):
DynamicCpp.__init__(self, name)
self.model = None
self.data = None
def setData(self, pinocchio_data):
dynamic.wrap.set_pinocchio_data(self.obj, pinocchio_data) # noqa TODO
self.data = pinocchio_data
return
def setModel(self, pinocchio_model):
dynamic.wrap.set_pinocchio_model(self.obj, pinocchio_model) # noqa TODO
self.model = pinocchio_model
return
def fromSotToPinocchio(q_sot, freeflyer=True): def fromSotToPinocchio(q_sot, freeflyer=True):
if freeflyer: if freeflyer:
......
...@@ -271,9 +271,12 @@ class AbstractRobot(ABC): ...@@ -271,9 +271,12 @@ class AbstractRobot(ABC):
""" """
Half sitting configuration. Half sitting configuration.
""" """
self.halfSitting = numpy.asarray(pinocchio.neutral(self.pinocchioModel)).flatten().tolist() self.halfSitting = pinocchio.neutral(self.pinocchioModel)
self.defineHalfSitting(self.halfSitting) self.defineHalfSitting(self.halfSitting)
self.halfSitting[3:7] = [0., 0., 0.] # Replace quaternion by RPY. self.halfSitting = numpy.array(self.halfSitting[:3].tolist()
+ [0., 0., 0.] # Replace quaternion by RPY.
+ self.halfSitting[7:].tolist())
assert self.halfSitting.shape[0] == self.dynamic.getDimension()
# Set the device limits. # Set the device limits.
def get(s): def get(s):
...@@ -283,9 +286,10 @@ class AbstractRobot(ABC): ...@@ -283,9 +286,10 @@ class AbstractRobot(ABC):
def opposite(v): def opposite(v):
return [-x for x in v] return [-x for x in v]
self.device.setPositionBounds(get(self.dynamic.lowerJl), get(self.dynamic.upperJl)) self.dynamic.add_signals()
self.device.setVelocityBounds(opposite(get(self.dynamic.upperVl)), get(self.dynamic.upperVl)) self.device.setPositionBounds( get(self.dynamic.lowerJl), get(self.dynamic.upperJl))
self.device.setTorqueBounds(opposite(get(self.dynamic.upperTl)), get(self.dynamic.upperTl)) self.device.setVelocityBounds(-get(self.dynamic.upperVl), get(self.dynamic.upperVl))
self.device.setTorqueBounds (-get(self.dynamic.upperTl), get(self.dynamic.upperTl))
# Freeflyer reference frame should be the same as global # Freeflyer reference frame should be the same as global
# frame so that operational point positions correspond to # frame so that operational point positions correspond to
...@@ -299,7 +303,7 @@ class AbstractRobot(ABC): ...@@ -299,7 +303,7 @@ class AbstractRobot(ABC):
plug(self.device.state, self.velocityDerivator.sin) plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity) plug(self.velocityDerivator.sout, self.dynamic.velocity)
else: else:
self.dynamic.velocity.value = self.dimension * (0., ) self.dynamic.velocity.value = numpy.zeros([self.dimension,])
if self.enableAccelerationDerivator: if self.enableAccelerationDerivator:
self.accelerationDerivator = \ self.accelerationDerivator = \
...@@ -308,7 +312,7 @@ class AbstractRobot(ABC): ...@@ -308,7 +312,7 @@ class AbstractRobot(ABC):
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin) plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration) plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else: else:
self.dynamic.acceleration.value = self.dimension * (0., ) self.dynamic.acceleration.value = numpy.zeros([self.dimension,])
def addTrace(self, entityName, signalName): def addTrace(self, entityName, signalName):
if self.tracer: if self.tracer:
......
#include <sot/dynamic-pinocchio/integrator-force-exact.h>
typedef boost::mpl::vector< dynamicgraph::sot::IntegratorForceExact > entities_t;
#include <sot/dynamic-pinocchio/integrator-force.h>
typedef boost::mpl::vector< dynamicgraph::sot::IntegratorForce > entities_t;
#include <sot/dynamic-pinocchio/integrator-force-rk4.h>
typedef boost::mpl::vector< dynamicgraph::sot::IntegratorForceRK4 > entities_t;
#include <sot/dynamic-pinocchio/mass-apparent.h>
typedef boost::mpl::vector< dynamicgraph::sot::MassApparent > entities_t;
// Copyright (C) 2008-2016, 2019 LAAS-CNRS, JRL AIST-CNRS.
#define PY_SSIZE_T_CLEAN
#include <sot/core/debug.hh>
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
#include <Python.h>
#include <boost/python.hpp>
#include <typeinfo>
#include <cstdio>
namespace dynamicgraph {
namespace sot {
PyObject* setPinocchioModel(PyObject* /* self */, PyObject* args) {
PyObject* object = NULL;
PyObject* pyPinocchioObject;
void* pointer1 = NULL;
pinocchio::Model* pointer2 = NULL;
if (!PyArg_ParseTuple(args, "OO", &object, &pyPinocchioObject)) return NULL;
if (!PyCapsule_CheckExact(object)) {
PyErr_SetString(PyExc_TypeError, "function takes a PyCapsule as argument");
return NULL;
}
pointer1 = PyCapsule_GetPointer(object, "dynamic_graph.Entity");
DynamicPinocchio* dyn_entity = (DynamicPinocchio*)pointer1;
try {
boost::python::extract<pinocchio::Model&> cppHandle(pyPinocchioObject);
pointer2 = (pinocchio::Model*)&cppHandle();
dyn_entity->setModel(pointer2);
} catch (const std::exception& exc) {
// PyErr_SetString(dgpyError, exc.what());
return NULL;
} catch (const char* s) {
// PyErr_SetString(dgpyError, s);
return NULL;
} catch (...) {
// PyErr_SetString(dgpyError, "Unknown exception");
return NULL;
}
return Py_BuildValue("");
}
PyObject* setPinocchioData(PyObject* /* self */, PyObject* args) {
PyObject* object = NULL;
PyObject* pyPinocchioObject;
void* pointer1 = NULL;
pinocchio::Data* pointer2 = NULL;
if (!PyArg_ParseTuple(args, "OO", &object, &pyPinocchioObject)) return NULL;
if (!PyCapsule_CheckExact(object)) {
PyErr_SetString(PyExc_TypeError, "function takes a PyCapsule as argument");
return NULL;
}
pointer1 = PyCapsule_GetPointer(object, "dynamic_graph.Entity");
DynamicPinocchio* dyn_entity = (DynamicPinocchio*)pointer1;
try {
boost::python::extract<pinocchio::Data&> cppHandle(pyPinocchioObject);
pointer2 = (pinocchio::Data*)&cppHandle();
dyn_entity->setData(pointer2);
} catch (const std::exception& exc) {
// PyErr_SetString(dgpyError, exc.what());
return NULL;
} catch (const char* s) {
// PyErr_SetString(dgpyError, s);
return NULL;
} catch (...) {
// PyErr_SetString(dgpyError, "Unknown exception");
return NULL;
}
return Py_BuildValue("");
}
} // namespace sot
} // namespace dynamicgraph
/**
\brief List of python functions
*/
static PyMethodDef functions[] = {
/* {"get_pinocchio_model", dynamicgraph::sot::getPinocchioModel, METH_VARARGS,
"Get the pinocchio model as python object"},*/
{"set_pinocchio_model", dynamicgraph::sot::setPinocchioModel, METH_VARARGS,
"Set the model from pinocchio python object"},
{"set_pinocchio_data", dynamicgraph::sot::setPinocchioData, METH_VARARGS,
"Set the data from pinocchio python object"},
{NULL, NULL, 0, NULL} /* Sentinel */
};
#if PY_MAJOR_VERSION >= 3
static struct PyModuleDef SotDynamicPinocchioModuleDef = {
PyModuleDef_HEAD_INIT, "wrap", NULL, 0, functions, NULL, NULL, NULL, NULL};
#define INITERROR return NULL
#else
#define INITERROR return
#endif
#ifdef __cplusplus
extern "C" {
#endif
#if PY_MAJOR_VERSION >= 3
PyMODINIT_FUNC PyInit_wrap(void)
#else
void initwrap(void)
#endif
{
#if PY_MAJOR_VERSION >= 3
PyObject* module = PyModule_Create(&SotDynamicPinocchioModuleDef);
#else
PyObject* module = Py_InitModule("wrap", functions);
#endif
if (module == NULL) INITERROR;
#if PY_MAJOR_VERSION >= 3
return module;
#endif
}
#ifdef __cplusplus
} // extern "C"
#endif
...@@ -35,7 +35,7 @@ const std::string dg::sot::DynamicPinocchio::CLASS_NAME = "DynamicPinocchio"; ...@@ -35,7 +35,7 @@ const std::string dg::sot::DynamicPinocchio::CLASS_NAME = "DynamicPinocchio";
DynamicPinocchio::DynamicPinocchio(const std::string& name) DynamicPinocchio::DynamicPinocchio(const std::string& name)
: Entity(name), : Entity(name),
m_model(NULL), m_model(NULL),
m_data(NULL) m_data()
, ,
jointPositionSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::position"), jointPositionSIN(NULL, "sotDynamicPinocchio(" + name + ")::input(vector)::position"),
...@@ -241,9 +241,15 @@ void DynamicPinocchio::setModel(pinocchio::Model* modelPtr) { ...@@ -241,9 +241,15 @@ void DynamicPinocchio::setModel(pinocchio::Model* modelPtr) {
if (pinocchio::nq(this->m_model->joints[i]) == 4) // Spherical Joint Only if (pinocchio::nq(this->m_model->joints[i]) == 4) // Spherical Joint Only
sphericalJoints.push_back(pinocchio::idx_v(this->m_model->joints[i])); sphericalJoints.push_back(pinocchio::idx_v(this->m_model->joints[i]));
} }
createData();
} }
void DynamicPinocchio::setData(pinocchio::Data* dataPtr) { this->m_data = dataPtr; } void DynamicPinocchio::setData(pinocchio::Data*) {}
void DynamicPinocchio::createData() {
m_data.reset(new pinocchio::Data (*m_model));
}
/*--------------------------------GETTERS-------------------------------------------*/ /*--------------------------------GETTERS-------------------------------------------*/
......
#include <sot/dynamic-pinocchio/waist-attitude-from-sensor.h>
typedef boost::mpl::vector< dynamicgraph::sot::WaistAttitudeFromSensor > entities_t;
#include "zmp-from-forces.h"
typedef boost::mpl::vector< sot::dynamic::ZmpFromForces > entities_t;
...@@ -6,103 +6,10 @@ ...@@ -6,103 +6,10 @@
* *
*/ */
#include <vector> #include "zmp-from-forces.h"
#include <sstream>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
namespace sot { namespace sot {
namespace dynamic { namespace dynamic {
using dynamicgraph::Entity;
using dynamicgraph::SignalPtr;
using dynamicgraph::SignalTimeDependent;
using dynamicgraph::Vector;
using dynamicgraph::sot::MatrixHomogeneous;
class ZmpFromForces : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
static const unsigned int footNumber = 2;
ZmpFromForces(const std::string& name) : Entity(name), zmpSOUT_(CLASS_NAME + "::output(Vector)::zmp") {
zmpSOUT_.setFunction(boost::bind(&ZmpFromForces::computeZmp, this, _1, _2));
signalRegistration(zmpSOUT_);
for (unsigned int i = 0; i < footNumber; i++) {
std::ostringstream forceName, positionName;
forceName << CLASS_NAME << "::input(vector6)::force_" << i;
positionName << CLASS_NAME << "::input(MatrixHomo)::sensorPosition_" << i;
forcesSIN_[i] = new SignalPtr<Vector, int>(0, forceName.str());
sensorPositionsSIN_[i] = new SignalPtr<MatrixHomogeneous, int>(0, positionName.str());
signalRegistration(*forcesSIN_[i]);
signalRegistration(*sensorPositionsSIN_[i]);
zmpSOUT_.addDependency(*forcesSIN_[i]);
zmpSOUT_.addDependency(*sensorPositionsSIN_[i]);
}
}
virtual std::string getDocString() const {
std::string docstring =
"Compute ZMP from force sensor measures and positions\n"
"\n"
" Takes 4 signals as input:\n"
" - force_0: wrench measured by force sensor 0 as a 6 dimensional vector\n"
" - force_0: wrench measured by force sensor 1 as a 6 dimensional vector\n"
" - sensorPosition_0: position of force sensor 0\n"
" - sensorPosition_1: position of force sensor 1\n"
" \n"
" compute the Zero Momentum Point of the contact forces as measured by the \n"
" input signals under the asumptions that the contact points between the\n"
" robot and the environment are located in the same horizontal plane.\n";
return docstring;
}
private:
Vector& computeZmp(Vector& zmp, int time) {
double fnormal = 0;
double sumZmpx = 0;
double sumZmpy = 0;
double sumZmpz = 0;
zmp.resize(3);
for (unsigned int i = 0; i < footNumber; ++i) {
const Vector& f = forcesSIN_[i]->access(time);
// Check that force is of dimension 6
if (f.size() != 6) {
zmp.fill(0.);
return zmp;
}
const MatrixHomogeneous& M = sensorPositionsSIN_[i]->access(time);
double fx = M(0, 0) * f(0) + M(0, 1) * f(1) + M(0, 2) * f(2);
double fy = M(1, 0) * f(0) + M(1, 1) * f(1) + M(1, 2) * f(2);
double fz = M(2, 0) * f(0) + M(2, 1) * f(1) + M(2, 2) * f(2);
if (fz > 0) {
double Mx = M(0, 0) * f(3) + M(0, 1) * f(4) + M(0, 2) * f(5) + M(1, 3) * fz - M(2, 3) * fy;
double My = M(1, 0) * f(3) + M(1, 1) * f(4) + M(1, 2) * f(5) + M(2, 3) * fx - M(0, 3) * fz;
fnormal += fz;
sumZmpx -= My;
sumZmpy += Mx;
sumZmpz += fz * M(2, 3);
}
}