Commit 35f0e53f authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

Merge branch 'topic/sot-pinocchio'

[python] Remove Pinocchio wrapper and take pinocchio model and data directly as input
[python] Update Class HumanoidRobot
[c++] Deal with spherical Joints
[c++] Add kineromeo.py tutorial (uses gepetto-viewer for visualization)

Conflicts:
	CMakeLists.txt
parents b8621cdf e82f73f6
*.pyc
*~
build
.gitignore
......@@ -8,16 +8,22 @@ after_success:
branches:
only:
- master
- topic/sot-pinocchio
compiler:
- clang
- gcc
matrix:
allow_failures:
- compiler: clang
before_install:
- git submodule update --init --recursive
#Add robotpkg and pinocchio dependencies
- sudo sh -c "echo \"deb http://robotpkg.openrobots.org/packages/debian precise robotpkg\" >> /etc/apt/sources.list "
- curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
- sudo apt-get update -qq
- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev python-sphinx python-numpy
- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev
python-sphinx python-numpy libtinyxml-dev robotpkg-console-bridge robotpkg-urdfdom-headers robotpkg-urdfdom robotpkg-eigenpy
- sudo pip install cpp-coveralls
language: cpp
notifications:
......
......@@ -14,10 +14,11 @@ rm -rf "$build_dir" "$install_dir"
mkdir -p "$build_dir"
mkdir -p "$install_dir"
# Setup environment variables.
export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$install_dir/lib:/opt/openrobots/lib:$LD_LIBRARY_PATH"
export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH"
export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH"
install_dependency()
......@@ -31,11 +32,18 @@ install_dependency()
make install
}
# Retrieve jrl-mathtools
install_dependency jrl-umi3218/jrl-mathtools
install_dependency jrl-umi3218/jrl-mal
install_dependency laas/abstract-robot-dynamics
install_dependency jrl-umi3218/jrl-dynamics
install_pinocchio_devel()
{
echo "--> Compiling stack-of-tasks/pinocchio"
mkdir -p "$build_dir/stack-of-tasks/pinocchio"
cd "$build_dir"
$git_clone -b devel git://github.com/stack-of-tasks/pinocchio stack-of-tasks/pinocchio
cd "$build_dir/stack-of-tasks/pinocchio"
cmake . -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
make install
}
install_pinocchio_devel
install_dependency proyan/dynamic-graph
install_dependency proyan/dynamic-graph-python
install_dependency proyan/sot-core
......
......@@ -23,8 +23,8 @@ INCLUDE(cmake/cpack.cmake)
SET(PROJECT_NAME sot-dynamic)
SET(PROJECT_DESCRIPTION "jrl-dynamics bindings for dynamic-graph.")
SET(PROJECT_URL "http://github.com/stack-of-tasks/sot-dynamic")
SET(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.")
SET(PROJECT_URL "https://github.com/proyan/sot-dynamic/tree/topic/sot-pinocchio")
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
......@@ -38,9 +38,9 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
)
SETUP_PROJECT()
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.19.0")
ADD_REQUIRED_DEPENDENCY("pinocchio")
ADD_REQUIRED_DEPENDENCY("eigenpy")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
......@@ -69,7 +69,7 @@ PKG_CONFIG_APPEND_LIBS(${LIBRARY_NAME})
# Search for dependencies.
# Boost
SET(BOOST_COMPONENTS filesystem system)
SET(BOOST_COMPONENTS filesystem system unit_test_framework python)
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
......@@ -78,7 +78,7 @@ ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(doc)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unitTesting)
#ADD_SUBDIRECTORY(unitTesting)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
sot-dynamic
===========
[![Build Status](https://travis-ci.org/stack-of-tasks/sot-dynamic.png?branch=master)](https://travis-ci.org/stack-of-tasks/sot-dynamic)
[![Coverage Status](https://coveralls.io/repos/stack-of-tasks/sot-dynamic/badge.png)](https://coveralls.io/r/stack-of-tasks/sot-dynamic)
[![Build Status](https://travis-ci.org/proyan/sot-dynamic.png?branch=master)](https://travis-ci.org/proyan/sot-dynamic)
[![Coverage Status](https://coveralls.io/repos/proyan/sot-dynamic/badge.png)](https://coveralls.io/r/proyan/sot-dynamic)
This software provides robot dynamic computation for dynamic-graph
by using jrl-dynamics.
......@@ -28,18 +28,15 @@ The matrix abstract layer depends on several packages which
have to be available on your machine.
- Libraries:
- [jrl-dynamics][jrl-dynamics] (>= 1.16.1)
- [dynamic-graph][dynamic-graph] (>= 3.0.0)
- [sot-core][sot-core] (>= 3.0.0)
- Closed source libraries:
- hrp2Dynamics (>= 1.3.0)
- hrp2-10-optimized (>= 1.3.0) [optional]
- [pinocchio][pinocchio] (>= 1.1.2)
- System tools:
- CMake (>=2.6)
- pkg-config
- usual compilation tools (GCC/G++, make, etc.)
[dynamic-graph]: http://github.com/stack-of-tasks/dynamic-graph
[jrl-dynamics]: http://github.com/jrl-umi3218/jrl-dynamics
[sot-core]: http://github.com/stack-of-tasks/sot-core
[dynamic-graph]: http://github.com/proyan/dynamic-graph
[pinocchio]: http://github.com/stack-of-tasks/pinocchio
[sot-core]: http://github.com/proyan/sot-core
# Copyright (C) 2008-2016 LAAS-CNRS, JRL AIST-CNRS.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
# SOT_DYNAMIC_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME
# ---------------------------
#
# Add a python submodule to dynamic_graph
#
# SUBMODULENAME : the name of the submodule (can be foo/bar),
#
# LIBRARYNAME : library to link the submodule with.
#
# TARGETNAME : name of the target: should be different for several
# calls to the macro.
#
# NOTICE : Before calling this macro, set variable NEW_ENTITY_CLASS as
# the list of new Entity types that you want to be bound.
# Entity class name should match the name referencing the type
# in the factory.
#
MACRO(SOT_DYNAMIC_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
FINDPYTHON()
SET(PYTHON_MODULE ${TARGETNAME})
ADD_LIBRARY(${PYTHON_MODULE}
MODULE
${PROJECT_SOURCE_DIR}/src/python-module-py.cpp)
#${PROJECT_SOURCE_DIR}/src/sot-dynamic-py.cpp)
FILE(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME})
SET_TARGET_PROPERTIES(${PYTHON_MODULE}
PROPERTIES PREFIX ""
OUTPUT_NAME dynamic_graph/${SUBMODULENAME}/wrap
)
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} "-Wl,--no-as-needed")
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${LIBRARYNAME} ${PYTHON_LIBRARY})
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${Boost_LIBRARIES})
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} pinocchio)
TARGET_LINK_LIBRARIES(${PYTHON_MODULE} eigenpy)
PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${PYTHON_MODULE} eigenpy)
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
#
# Installation
#
SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/dynamic_graph/${SUBMODULENAME})
INSTALL(TARGETS ${PYTHON_MODULE}
DESTINATION
${PYTHON_INSTALL_DIR})
SET(ENTITY_CLASS_LIST "")
FOREACH (ENTITY ${NEW_ENTITY_CLASS})
SET(ENTITY_CLASS_LIST "${ENTITY_CLASS_LIST}${ENTITY}('')\n")
ENDFOREACH(ENTITY ${NEW_ENTITY_CLASS})
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/cmake/dynamic_graph/submodule/__init__.py.cmake
${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME}/__init__.py
)
INSTALL(
FILES ${PROJECT_BINARY_DIR}/src/dynamic_graph/${SUBMODULENAME}/__init__.py
DESTINATION ${PYTHON_INSTALL_DIR}
)
ENDMACRO(DYNAMIC_GRAPH_PYTHON_MODULE SUBMODULENAME)
\ No newline at end of file
This diff is collapsed.
......@@ -29,13 +29,6 @@
#include <sot/core/matrix-geometry.hh>
namespace dynamicsJRLJapan
{
class HumanoidDynamicMultiBody;
}
class CjrlHumanoidDynamicRobot;
class CjrlJoint;
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
......
......@@ -14,6 +14,7 @@
# received a copy of the GNU Lesser General Public License along with
# sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
INCLUDE(../custom_cmake/python.cmake)
INCLUDE(../cmake/python.cmake)
LINK_DIRECTORIES(${Boost_LIBRARY_DIRS})
......@@ -29,7 +30,6 @@ ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
SET(integrator-force-rk4_plugins_dependencies integrator-force)
SET(integrator-force-exact_plugins_dependencies integrator-force)
FOREACH(lib ${plugins})
ADD_LIBRARY(${lib} SHARED ${lib}.cpp)
......@@ -44,7 +44,8 @@ FOREACH(lib ${plugins})
TARGET_LINK_LIBRARIES(${lib} ${Boost_LIBRARIES})
PKG_CONFIG_USE_DEPENDENCY(${lib} jrl-dynamics)
PKG_CONFIG_USE_DEPENDENCY(${lib} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${lib} eigenpy)
PKG_CONFIG_USE_DEPENDENCY(${lib} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${lib} dynamic-graph)
......@@ -53,7 +54,7 @@ FOREACH(lib ${plugins})
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/dynamics/${PYTHON_LIBRARY_NAME}"
SOT_DYNAMIC_PYTHON_MODULE("sot/dynamics/${PYTHON_LIBRARY_NAME}"
${lib}
sot-dynamics-${PYTHON_LIBRARY_NAME}-wrap
)
......@@ -61,13 +62,13 @@ ENDFOREACH(lib)
# Main Library
ADD_LIBRARY(${LIBRARY_NAME} SHARED sot-dynamic.cpp)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} jrl-dynamics)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} pinocchio)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} sot-core)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} jrl-dynamics)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
TARGET_LINK_LIBRARIES(dynamic ${LIBRARY_NAME})
......@@ -79,5 +80,4 @@ INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/tools.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/parser.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dynamics
)
)
\ No newline at end of file
......@@ -35,7 +35,10 @@ namespace dynamicgraph { namespace sot {
class FromSensor : public Command
{
public:
virtual ~FromSensor() {}
virtual ~FromSensor() {
sotDEBUGIN(15);
sotDEBUGOUT(15);
}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
......
......@@ -32,124 +32,16 @@ namespace dynamicgraph { namespace sot {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
// Command SetFiles
class SetFiles : public Command
// Command DisplayModel
class DisplayModel : public Command
{
public:
virtual ~SetFiles() {}
virtual ~DisplayModel() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetFiles(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)
(Value::STRING)(Value::STRING)(Value::STRING), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string vrmlDirectory = values[0].value();
std::string vrmlMainFile = values[1].value();
std::string xmlSpecificityFiles = values[2].value();
std::string xmlRankFile = values[3].value();
robot.setVrmlDirectory(vrmlDirectory);
robot.setVrmlMainFile(vrmlMainFile);
robot.setXmlSpecificityFile(xmlSpecificityFiles);
robot.setXmlRankFile(xmlRankFile);
// return void
return Value();
}
}; // class SetFiles
// Command Parse
class Parse : public Command
{
public:
virtual ~Parse() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
Parse(Dynamic& entity, const std::string& docstring) :
Command(entity, std::vector<Value::Type>(), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
if(! robot.init ) robot.parseConfigFiles();
else std::cout << " !! Already parsed." << std::endl;
// return void
return Value();
}
}; // class Parse
// Command SetProperty
class SetProperty : public Command
{
public:
virtual ~SetProperty() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetProperty(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string property = values[0].value();
std::string value = values[1].value();
robot.m_HDR->setProperty(property, value);
return Value();
}
}; // class SetProperty
// Command GetProperty
class GetProperty : public Command
{
public:
virtual ~GetProperty() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
GetProperty(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string property = values[0].value();
std::string value;
if(! robot.m_HDR->getProperty(property, value) )
{
if( property == "vrmlDirectory" ) value = robot.vrmlDirectory;
else if( property == "xmlSpecificityFile" ) value = robot.xmlSpecificityFile;
else if( property == "xmlRankFile" ) value = robot.xmlRankFile;
else if( property == "vrmlMainFile" ) value = robot.vrmlMainFile;
}
return Value(value);
}
}; // class GetProperty
// Command CreateRobot
class CreateRobot : public Command
{
public:
virtual ~CreateRobot() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
CreateRobot(Dynamic& entity, const std::string& docstring) :
DisplayModel(Dynamic& entity, const std::string& docstring) :
Command(entity, std::vector<Value::Type>(),
docstring)
{
......@@ -157,307 +49,18 @@ namespace dynamicgraph { namespace sot {
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
robot.createRobot();
return Value();
}
}; // class CreateRobot
// Command CreateJoint
class CreateJoint : public Command
{
public:
virtual ~CreateJoint() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
CreateJoint(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)
(Value::MATRIX), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
std::string jointType = values[1].value();
dynamicgraph::Matrix position = values[2].value();
robot.createJoint(jointName, jointType, position);
return Value();
}
}; // class CreateJoint
// Command SetRootJoint
class SetRootJoint : public Command
{
public:
virtual ~SetRootJoint() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetRootJoint(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
robot.setRootJoint(jointName);
return Value();
}
}; // class SetRootJoint
// Command AddJoint
class AddJoint : public Command
{
public:
virtual ~AddJoint() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
AddJoint(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string parentName = values[0].value();
std::string childName = values[1].value();
robot.addJoint(parentName, childName);
return Value();
}
}; // class AddJoint
// Command SetDofBounds
class SetDofBounds : public Command
{
public:
virtual ~SetDofBounds() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetDofBounds(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::UNSIGNED)
(Value::DOUBLE)(Value::DOUBLE), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
unsigned int dofId = values[1].value();
double minValue = values[2].value();
double maxValue = values[3].value();
robot.setDofBounds(jointName, dofId, minValue, maxValue);
return Value();
}
}; // class SetDofBounds
// Command SetMass
class SetMass : public Command
{
public:
virtual ~SetMass() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetMass(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::DOUBLE),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
double mass = values[1].value();
robot.setMass(jointName, mass);
return Value();
}
}; // class SetMass
// Command SetLocalCenterOfMass
class SetLocalCenterOfMass : public Command
{
public:
virtual ~SetLocalCenterOfMass() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetLocalCenterOfMass(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::VECTOR),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
dynamicgraph::Vector com = values[1].value();
robot.setLocalCenterOfMass(jointName, com);
return Value();
}
}; // class SetLocalCenterOfMass
// Command SetInertiaMatrix
class SetInertiaMatrix : public Command
{
public:
virtual ~SetInertiaMatrix() {}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
SetInertiaMatrix(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)(Value::MATRIX),
docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string jointName = values[0].value();
dynamicgraph::Matrix