...
 
Commits (17)
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
SET(PROJECT_NAMESPACE stack-of-tasks)
# Project properties
SET(PROJECT_ORG stack-of-tasks)
SET(PROJECT_NAME sot-dynamic-pinocchio)
SET(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.")
SET(PROJECT_URL "https://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
SET(PROJECT_SUFFIX "-v3")
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
# Project options
OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
# Project configuration
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
SET(CUSTOM_HEADER_DIR "sot/dynamic-pinocchio")
SET(CXX_DISABLE_WERROR TRUE)
SET(DOXYGEN_USE_MATHJAX YES)
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES}
plugindirname
plugindir
)
# JRL-cmakemodule setup
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/sphinx.cmake)
# Project definition
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 4.1.0")
ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
# Project dependencies
ADD_PROJECT_DEPENDENCY(sot-tools REQUIRED PKG_CONFIG_REQUIRES sot-tools)
SET(BOOST_COMPONENTS filesystem system unit_test_framework)
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.4.0")
STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR})
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED
PKG_CONFIG_REQUIRES dynamic-graph-python)
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_PATH})
ADD_REQUIRED_DEPENDENCY("eigenpy")
ENDIF(BUILD_PYTHON_INTERFACE)
# Add dependency toward sot-dynamic-pinocchio library in pkg-config file.
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
# Add subdirectories.
# Main Library
SET(${PROJECT_NAME}_HEADERS
include/${CUSTOM_HEADER_DIR}/dynamic-pinocchio.h
include/${CUSTOM_HEADER_DIR}/integrator-force-exact.h
include/${CUSTOM_HEADER_DIR}/zmpreffromcom.h
include/${CUSTOM_HEADER_DIR}/integrator-force.h
include/${CUSTOM_HEADER_DIR}/force-compensation.h
include/${CUSTOM_HEADER_DIR}/mass-apparent.h
include/${CUSTOM_HEADER_DIR}/waist-attitude-from-sensor.h
include/${CUSTOM_HEADER_DIR}/matrix-inertia.h
include/${CUSTOM_HEADER_DIR}/integrator-force-rk4.h
include/${CUSTOM_HEADER_DIR}/angle-estimator.h
)
SET(${PROJECT_NAME}_SOURCES
src/sot-dynamic-pinocchio
)
ADD_LIBRARY(${PROJECT_NAME} SHARED
${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES}
sot-core::sot-core)
IF(SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
ENDIF(SUFFIX_SO_VERSION)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
ADD_SUBDIRECTORY(include/${CUSTOM_HEADER_DIR})
INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(doc)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unitTesting)
ADD_SUBDIRECTORY(tests)
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
Subproject commit 811bfc472940425a0a96cca7e01331f6cf68fced
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
#
# Copyright 2010 CNRS
# Author: Florent Lamiraux
#
INCLUDE(../cmake/sphinx.cmake)
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
SET(${PROJECT_NAME}_HEADERS
dynamic-pinocchio.h
integrator-force-exact.h
zmpreffromcom.h
integrator-force.h
force-compensation.h
mass-apparent.h
waist-attitude-from-sensor.h
matrix-inertia.h
integrator-force-rk4.h
angle-estimator.h
)
INSTALL(FILES ${${PROJECT_NAME}_HEADERS} DESTINATION include/${CUSTOM_HEADER_DIR})
......@@ -23,7 +23,7 @@
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
#include <sot-dynamic-pinocchio/integrator-force.h>
#include <sot/dynamic-pinocchio/integrator-force.h>
/* STD */
#include <string>
......
......@@ -23,7 +23,7 @@
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
#include <sot-dynamic-pinocchio/integrator-force.h>
#include <sot/dynamic-pinocchio/integrator-force.h>
/* STD */
#include <string>
......
<package format="2">
<name>sot-dynamic-pinocchio</name>
<version>3.4.5</version>
<description>
Pinocchio bindings for dynamic-graph
</description>
<maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
<license>BSD</license>
<url type="website">http://stack-of-tasks.github.io</url>
<url type="repository">http://www.github.com/stack-of-tasks/sot-dynamic-pinocchio</url>
<url type="bugtracker">http://www.github.com/stack-of-tasks/sot-dynamic-pinocchio/issues</url>
<author>Olivier Stasse</author>
<author email="ostasse@laas.fr">ostasse@laas.fr</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend version_gte="1.1" version_lt="2.0">genmsg</build_depend>
<build_depend>sot-core</build_depend>
<build_depend>pinocchio</build_depend>
<build_depend>sot-core</build_depend>
<build_depend>pinocchio</build_depend>
<build_export_depend>pinocchio</build_export_depend>
<exec_depend>sot-core</exec_depend>
<exec_depend>pinocchio</exec_depend>
<test_depend>gtest</test_depend>
<doc_depend>doxygen</doc_depend>
<doc_depend>dynamic_graph</doc_depend>
<export>
</export>
</package>
SET(${PROJECT_NAME}_PYTHON
__init__.py
kine_romeo.py
kine_romeo_small.py
)
IF(BUILD_PYTHON_INTERFACE)
INSTALL(
FILES kine_romeo.py kine_romeo_small.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/tutorial
)
FOREACH(file ${${PROJECT_NAME}_PYTHON})
PYTHON_INSTALL_ON_SITE("dynamic_graph/${PYTHON_DIR}" ${file})
ENDFOREACH(file ${${PROJECT_NAME}_PYTHON})
ENDIF(BUILD_PYTHON_INTERFACE)
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST,
# Florent Lamiraux (CNRS/LAAS)
# Copyright 2010, 2020, JRL, CNRS/AIST, LAAS-CNRS
# François Bleibel
# Olivier Stasse
# Florent Lamiraux
# Guilhem Saurel
# Main Library
ADD_LIBRARY(${PROJECT_NAME} SHARED sot-dynamic-pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} dynamic-graph)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES})
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
# List plug-ins that will be compiled.
SET(plugins
zmpreffromcom
force-compensation
......@@ -26,54 +17,42 @@ SET(plugins
dynamic
)
SET(integrator-force-rk4_plugins_dependencies integrator-force)
SET(integrator-force-exact_plugins_dependencies integrator-force)
FOREACH(lib ${plugins})
SET(libname dp-${lib})
ADD_LIBRARY(${libname} SHARED ${lib})
SET_TARGET_PROPERTIES(${libname} PROPERTIES
PREFIX ""
SOVERSION ${PROJECT_VERSION})
SET(integrator-force-rk4_plugins_deps integrator-force)
SET(integrator-force-exact_plugins_deps integrator-force)
IF(${lib}_plugins_dependencies)
ADD_DEPENDENCIES(${libname} "dp-${${lib}_plugins_dependencies}")
TARGET_LINK_LIBRARIES(${libname} "dp-${${lib}_plugins_dependencies}")
ENDIF(${lib}_plugins_dependencies)
FOREACH(plugin ${plugins})
GET_FILENAME_COMPONENT(LIBRARY_NAME ${plugin} NAME)
ADD_LIBRARY(${LIBRARY_NAME} SHARED ${plugin})
TARGET_LINK_LIBRARIES(${libname} ${PROJECT_NAME})
TARGET_LINK_LIBRARIES(${libname} ${Boost_LIBRARIES})
IF(SUFFIX_SO_VERSION)
SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
ENDIF(SUFFIX_SO_VERSION)
PKG_CONFIG_USE_DEPENDENCY(${libname} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${libname} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${libname} dynamic-graph)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${PROJECT_NAME} ${${LIBRARY_NAME}_deps})
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(TARGETS ${libname} DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
INSTALL(TARGETS ${LIBRARY_NAME} EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
# build python submodule
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${libname} eigenpy)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/dynamics_pinocchio/${PYTHON_LIBRARY_NAME}"
${libname}
sot-dynamic-pinocchio-${PYTHON_LIBRARY_NAME}-wrap
1
"src/python-module-py.cpp"
)
PKG_CONFIG_USE_DEPENDENCY(sot-dynamic-pinocchio-${PYTHON_LIBRARY_NAME}-wrap pinocchio)
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
DYNAMIC_GRAPH_PYTHON_MODULE("${PYTHON_DIR}/${PYTHON_LIBRARY_NAME}"
${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
1 "src/python-module-py.cpp")
TARGET_LINK_BOOST_PYTHON(${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap)
ENDIF(BUILD_PYTHON_INTERFACE)
ENDFOREACH(lib)
ENDFOREACH(plugin)
# Install empty __init__.py files in intermediate directories.
IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics_pinocchio/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics_pinocchio/humanoid_robot.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics_pinocchio/tools.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics_pinocchio/parser.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dynamics_pinocchio
SET(${PY_NAME}_PYTHON
__init__.py
humanoid_robot.py
tools.py
parser.py
)
FOREACH(py_file ${${PY_NAME}_PYTHON})
PYTHON_INSTALL_ON_SITE("dynamic_graph/${PYTHON_DIR}" ${py_file})
ENDFOREACH(py_file ${${PY_NAME}_PYTHON})
ENDIF(BUILD_PYTHON_INTERFACE)
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/angle-estimator.h>
#include <sot/dynamic-pinocchio/angle-estimator.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command.h>
......
......@@ -60,6 +60,31 @@ class GetDimension : public Command {
}
}; // class GetDimension
// Command getJointNames
class GetJointNames : public Command {
public:
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
GetJointNames(DynamicPinocchio& entity, const std::string& docstring)
: Command(entity, std::vector<Value::Type>(), docstring){}
virtual Value doExecute() {
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner());
if (robot.m_model == 0x0){
SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
"model has not been initialized.");
}
const std::vector<std::string>& jointNames = robot.m_model->names;
// Remove first joint names 'universe'
std::size_t n (jointNames.size());
assert(n >= 1);
std::vector<Value> res;
for (std::size_t i=1; i<jointNames.size(); ++i) {
res.push_back(Value(jointNames[i]));
}
return Value(res);
}
};
} // namespace command
} /* namespace sot */
} /* namespace dynamicgraph */
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
#include <dynamic-graph/factory.h>
using namespace dynamicgraph;
......
import numpy as np
from numpy import cos, sin, sqrt
from dynamic import DynamicPinocchio as DynamicCpp
from .dynamic import DynamicPinocchio as DynamicCpp
# DynamicOld = Dynamic
......
......@@ -6,10 +6,11 @@ from __future__ import print_function
from functools import reduce
from dynamic_graph import plug
from dynamic_graph.sot.core import OpPointModifier, RobotSimu
from dynamic_graph.sot.core.derivator import Derivator_of_Vector
from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamics_pinocchio.parser import Parser
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
from dynamic_graph.sot.core.robot_simu import RobotSimu
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamic_pinocchio.parser import Parser
from dynamic_graph.tools import addTrace
from dynamic_graph.tracer_real_time import TracerRealTime
......@@ -126,7 +127,7 @@ class AbstractHumanoidRobot(object):
"""
Load a model from a kxml file and return the parsed model.
This uses the Python parser class implement in
dynamic_graph.sot.dynamics_pinocchio.parser.
dynamic_graph.sot.dynamic_pinocchio.parser.
kxml is an extensible file format used by KineoWorks to store
both the robot mesh and its kinematic chain.
......
......@@ -5,7 +5,7 @@
import xml.dom.minidom as dom
from dynamic_graph.sot.dynamics_pinocchio.dynamic import DynamicPinocchio
from dynamic_graph.sot.dynamic_pinocchio.dynamic import DynamicPinocchio
from dynamic_graph.sot.tools.se3 import R3, SE3
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/force-compensation.h>
#include <sot/dynamic-pinocchio/force-compensation.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <sot/core/macros-signal.hh>
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/integrator-force-exact.h>
#include <sot/dynamic-pinocchio/integrator-force-exact.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <sot/core/exception-dynamic.hh>
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/integrator-force-rk4.h>
#include <sot/dynamic-pinocchio/integrator-force-rk4.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/integrator-force.h>
#include <sot/dynamic-pinocchio/integrator-force.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/mass-apparent.h>
#include <sot/dynamic-pinocchio/mass-apparent.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
......
......@@ -11,7 +11,7 @@
#include <vector>
#include <map>
#include <sot-dynamic-pinocchio/matrix-inertia.h>
#include <sot/dynamic-pinocchio/matrix-inertia.h>
//#include <jrl/dynamics/Joint.h>
//#include <jrl/dynamics/HumanoidDynamicMultiBody.h>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
......
......@@ -4,7 +4,7 @@
#include <sot/core/debug.hh>
#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
#include <Python.h>
#include <boost/python.hpp>
#include <typeinfo>
......
......@@ -9,7 +9,7 @@
#include <pinocchio/fwd.hpp>
#include <sot/core/debug.hh>
#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
#include <boost/version.hpp>
#include <boost/filesystem.hpp>
......@@ -202,6 +202,10 @@ DynamicPinocchio::DynamicPinocchio(const std::string& name)
"string (joint name)");
addCommand("createAcceleration",
makeCommandVoid2(*this, &DynamicPinocchio::cmd_createAccelerationSignal, docstring));
docstring =
"\n"
" Return robot joint names.\n\n";
addCommand("getJointNames", new command::GetJointNames(*this, docstring));
}
sphericalJoints.clear();
......@@ -633,14 +637,16 @@ dg::Vector& DynamicPinocchio::computeZmp(dg::Vector& res, const int& time) {
// Updates the jacobian matrix in m_data
int& DynamicPinocchio::computeJacobians(int& dummy, const int& time) {
sotDEBUGIN(25);
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
pinocchio::computeJointJacobians(*m_model, *m_data, q);
forwardKinematicsSINTERN(time);
pinocchio::computeJointJacobians(*m_model, *m_data);
sotDEBUG(25) << "Jacobians updated" << std::endl;
sotDEBUGOUT(25);
return dummy;
}
int& DynamicPinocchio::computeForwardKinematics(int& dummy, const int& time) {
sotDEBUGIN(25);
assert(m_model);
assert(m_data);
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
......@@ -665,84 +671,77 @@ dg::Matrix& DynamicPinocchio::computeGenericJacobian(const bool isFrame, const i
sotDEBUGIN(25);
assert(m_model);
assert(m_data);
if (res.rows() != 6 && res.cols() != m_model->nv) res.resize(6, m_model->nv);
if (res.rows() != 6 || res.cols() != m_model->nv) res = Matrix::Zero(6, m_model->nv);
jacobiansSINTERN(time);
// TODO: Find a way to remove tmp object
pinocchio::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6, m_model->nv);
pinocchio::JointIndex id =
isFrame ? m_model->frames[(pinocchio::JointIndex)jointId].parent : (pinocchio::JointIndex)jointId;
// Computes Jacobian in world coordinates.
if (isFrame) {
pinocchio::getJointJacobian(*m_model, *m_data, m_model->frames[(pinocchio::Model::Index)jointId].parent,
pinocchio::WORLD, tmp);
} else
pinocchio::getJointJacobian(*m_model, *m_data, (pinocchio::Model::Index)jointId, pinocchio::WORLD, tmp);
res = tmp;
pinocchio::getJointJacobian(*m_model, *m_data, id, pinocchio::WORLD, res);
sotDEBUGOUT(25);
return res;
}
dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int jointId,
dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int id,
dg::Matrix& res, const int& time) {
sotDEBUGIN(25);
assert(m_model);
assert(m_data);
if (res.rows() != 6 && res.cols() != m_model->nv) res.resize(6, m_model->nv);
if (res.rows() != 6 || res.cols() != m_model->nv) res = Matrix::Zero(6, m_model->nv);
jacobiansSINTERN(time);
forwardKinematicsSINTERN(time);
// TODO: Find a way to remove tmp object
pinocchio::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6, m_model->nv);
// std::string temp;
pinocchio::FrameIndex fid;
pinocchio::JointIndex jid;
bool changeFrame = !isLocal;
pinocchio::SE3 M;
// Computes Jacobian in end-eff coordinates.
if (isFrame) {
pinocchio::framesForwardKinematics(*m_model, *m_data);
pinocchio::getFrameJacobian(*m_model, *m_data, (pinocchio::Model::Index)jointId, pinocchio::LOCAL, tmp);
sotDEBUG(25) << "EndEffJacobian for " << m_model->frames.at((pinocchio::Model::Index)jointId).name << " is " << tmp
<< std::endl;
changeFrame = true;
fid = (pinocchio::FrameIndex)id;
const pinocchio::Frame& frame = m_model->frames[fid];
jid = frame.parent;
M = frame.placement.inverse();
if (!isLocal) // Express the jacobian is world coordinate system.
M.rotation() = m_data->oMf[fid].rotation() * M.rotation();
} else {
// temp = m_model->getJointName((pinocchio::Model::Index)jointId);
pinocchio::getJointJacobian(*m_model, *m_data, (pinocchio::Model::Index)jointId, pinocchio::LOCAL, tmp);
sotDEBUG(25) << "EndEffJacobian for " << m_model->names[(pinocchio::Model::Index)jointId] << " is " << tmp
<< std::endl;
jid = (pinocchio::JointIndex)id;
if (!isLocal) { // Express the jacobian is world coordinate system.
M.rotation() = m_data->oMi[jid].rotation();
M.translation().setZero();
}
}
res = tmp;
if (!isLocal) {
Eigen::Matrix3d rotation = (isFrame ? m_data->oMf : m_data->oMi)[jointId].rotation();
Eigen::Vector3d translation = Eigen::Vector3d::Zero();
pinocchio::getJointJacobian(*m_model, *m_data, jid, pinocchio::LOCAL, res);
res = (pinocchio::SE3(rotation, translation).toActionMatrix() * res);
}
if (changeFrame) pinocchio::motionSet::se3Action(M, res, res);
sotDEBUGOUT(25);
return res;
}
MatrixHomogeneous& DynamicPinocchio::computeGenericPosition(const bool isFrame, const int jointId,
MatrixHomogeneous& res, const int& time) {
MatrixHomogeneous& DynamicPinocchio::computeGenericPosition(const bool isFrame, const int id, MatrixHomogeneous& res,
const int& time) {
sotDEBUGIN(25);
assert(m_data);
std::string temp;
forwardKinematicsSINTERN(time);
if (isFrame) {
pinocchio::framesForwardKinematics(*m_model, *m_data);
res.matrix() = m_data->oMf[jointId].toHomogeneousMatrix();
temp = m_model->frames.at((pinocchio::Model::Index)jointId).name;
const pinocchio::Frame& frame = m_model->frames[id];
res.matrix() = (m_data->oMi[frame.parent] * frame.placement).toHomogeneousMatrix();
} else {
res.matrix() = m_data->oMi[jointId].toHomogeneousMatrix();
temp = m_model->names[(pinocchio::Model::Index)jointId];
res.matrix() = m_data->oMi[id].toHomogeneousMatrix();
}
sotDEBUG(25) << "For " << temp << " with id: " << jointId << " position is " << res << std::endl;
sotDEBUG(25) << "For " << (isFrame ? m_model->frames[id].name : m_model->names[id]) << " with id: " << id
<< " position is " << res << std::endl;
sotDEBUGOUT(25);
return res;
}
dg::Vector& DynamicPinocchio::computeGenericVelocity(const int jointId, dg::Vector& res, const int& time) {
sotDEBUGIN(25);
assert(m_data);
res.resize(6);
forwardKinematicsSINTERN(time);
res.resize(6);
const pinocchio::Motion& aRV = m_data->v[jointId];
res << aRV.linear(), aRV.angular();
sotDEBUGOUT(25);
......@@ -751,9 +750,8 @@ dg::Vector& DynamicPinocchio::computeGenericVelocity(const int jointId, dg::Vect
dg::Vector& DynamicPinocchio::computeGenericAcceleration(const int jointId, dg::Vector& res, const int& time) {
sotDEBUGIN(25);
assert(m_data);
res.resize(6);
forwardKinematicsSINTERN(time);
res.resize(6);
const pinocchio::Motion& aRA = m_data->a[jointId];
res << aRA.linear(), aRA.angular();
sotDEBUGOUT(25);
......@@ -779,17 +777,18 @@ int& DynamicPinocchio::computeNewtonEuler(int& dummy, const int& time) {
dg::Matrix& DynamicPinocchio::computeJcom(dg::Matrix& Jcom, const int& time) {
sotDEBUGIN(25);
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
Jcom = pinocchio::jacobianCenterOfMass(*m_model, *m_data, q, false);
forwardKinematicsSINTERN(time);
Jcom = pinocchio::jacobianCenterOfMass(*m_model, *m_data, false);
sotDEBUGOUT(25);
return Jcom;
}
dg::Vector& DynamicPinocchio::computeCom(dg::Vector& com, const int& time) {
sotDEBUGIN(25);
forwardKinematicsSINTERN(time);
pinocchio::centerOfMass(*m_model, *m_data, false);
if (JcomSOUT.needUpdate(time)) {
forwardKinematicsSINTERN(time);
pinocchio::centerOfMass(*m_model, *m_data, false);
}
com = m_data->com[0];
sotDEBUGOUT(25);
return com;
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/waist-attitude-from-sensor.h>
#include <sot/dynamic-pinocchio/waist-attitude-from-sensor.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
......
......@@ -7,7 +7,7 @@
*
*/
#include <sot-dynamic-pinocchio/zmpreffromcom.h>
#include <sot/dynamic-pinocchio/zmpreffromcom.h>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
using namespace dynamicgraph::sot;
......
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
SET(tests
test_constructor
#test_config
# dummy
# test_djj
# test_dyn
# test_results
)
test_constructor
#test_config
# dummy
# test_djj
# test_dyn
# test_results
)
SET(test_dyn_plugins_dependencies dynamic)
......@@ -28,31 +28,17 @@ ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
LIST(APPEND LOGGING_WATCHED_VARIABLES samplespec sampleljr)
FOREACH(test ${tests})
ADD_UNIT_TEST(${test} ${test})
ADD_UNIT_TEST(${test} ${test}.cpp)
TARGET_LINK_LIBRARIES(${test}
dp-zmpreffromcom
dp-force-compensation
${PROJECT_NAME} ${Boost_LIBRARIES} ${Boost_SYSTEM_LIBRARY} ${${test}_deps}
zmpreffromcom
force-compensation
#integrator-force-exact
dp-mass-apparent
dp-integrator-force-rk4
dp-integrator-force
dp-angle-estimator
dp-waist-attitude-from-sensor
${LIBRARY_NAME}
mass-apparent
integrator-force-rk4
integrator-force
angle-estimator
waist-attitude-from-sensor
)
PKG_CONFIG_USE_DEPENDENCY(${test} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${test} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${test} dynamic-graph)
IF(BUILD_PYTHON_INTERFACE)
TARGET_LINK_LIBRARIES(${test} dynamic-graph-python)
ENDIF(BUILD_PYTHON_INTERFACE)
IF(${test}_plugins_dependencies)
ADD_DEPENDENCIES(${EXECUTABLE_NAME} "${${test}_plugins_dependencies}")
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} "${${test}_plugins_dependencies}")
ENDIF(${test}_plugins_dependencies)
TARGET_LINK_LIBRARIES(${test} ${Boost_LIBRARIES} ${Boost_SYSTEM_LIBRARY})
ENDFOREACH(test)
......@@ -7,12 +7,6 @@
# ******************************************************************************
import pinocchio as pin
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
from numpy import eye
# -----------------------------------------------------------------------------
# ------------------------------------------------------------------------------
......@@ -22,6 +16,13 @@ from numpy import eye
# -----------------------------------------------------------------------------
from pinocchio.robot_wrapper import RobotWrapper
from dynamic_graph import plug
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import (MetaTaskKine6d, MetaTaskKineCom, gotoNd)
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.dynamic_pinocchio import fromSotToPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import HumanoidRobot
# -----------------------------------------------------------------------------
# SET THE PATH TO THE URDF AND MESHES
# Define robotName, urdfpath and initialConfig
......
......@@ -9,7 +9,7 @@
#include <boost/test/output_test_stream.hpp>
/*-----------SOT DYNAMIC ------------*/
#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
#include <sot/core/debug.hh>
/*-----------DYNAMIC GRAPH ------------*/
......
......@@ -10,7 +10,7 @@
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <sot-dynamic-pinocchio/dynamic-pinocchio.h>
#include <sot/dynamic-pinocchio/dynamic-pinocchio.h>
#include <iostream>
#include <sstream>
......