Add python script to easily load romeo.

CMakeLists.txt

+# Copyright 2013, Francois Keith, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
+#
+# This file is part of sot-romeo.
+# sot-romeo is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# sot-romeo 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 Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# sot-romeo. If not, see <http://www.gnu.org/licenses/>.
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/lapack.cmake)
+INCLUDE(cmake/cpack.cmake)
+INCLUDE(cmake/ros.cmake)
+
+SET(PROJECT_NAME sot-romeo)
+SET(PROJECT_DESCRIPTION "dynamic-graph package for Romeo robot")
+SET(PROJECT_URL "")
+
+SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
+
+SET(CXX_DISABLE_WERROR True)
+
+SET(PKG_CONFIG_ADDITIONAL_VARIABLES
+  ${PKG_CONFIG_ADDITIONAL_VARIABLES}
+  plugindirname
+  plugindir
+  )
+
+SETUP_PROJECT()
+
+# Search for dependencies.
+ADD_REQUIRED_DEPENDENCY("jrl-mathtools")
+ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.9.0")
+
+ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.19.0")
+ADD_REQUIRED_DEPENDENCY("romeo")
+
+ADD_REQUIRED_DEPENDENCY("dynamic-graph")
+ADD_REQUIRED_DEPENDENCY("sot-core")
+ADD_REQUIRED_DEPENDENCY("sot-dynamic")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
+
+ADD_ROSPACK_DEPENDENCY("dynamic_graph_bridge")
+# Search for dependencies.
+# Boost
+SET(BOOST_COMPONENTS filesystem system)
+SEARCH_FOR_BOOST()
+
+# Handle rpath necessary to handle ROS multiplace packages
+# libraries inclusion
+SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
+SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+# Add subdirectories.
+ADD_SUBDIRECTORY(src)
+
+SETUP_PROJECT_FINALIZE()
+SETUP_PROJECT_CPACK()

src/CMakeLists.txt

+# Copyright 2013, Francois Keith, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
+#
+# This file is part of sot-romeo.
+# sot-romeo is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# sot-romeo 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 Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# sot-romeo. If not, see <http://www.gnu.org/licenses/>.
+
+INCLUDE(../cmake/python.cmake)
+FINDPYTHON()
+INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
+
+CONFIG_FILES(dynamic_graph/sot/romeo/romeo.py)
+
+# Install Python files.
+SET(PYTHON_MODULE_DIR
+  ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/romeo)
+SET(PYTHON_MODULE_BUILD_DIR
+  ${CMAKE_CURRENT_BINARY_DIR}/dynamic_graph/sot/romeo)
+
+SET(PYTHON_MODULE dynamic_graph/sot/romeo)
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
+PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "romeo.py" "${PYTHON_SITELIB}")
+SET(FILES __init__.py romeo.py)
+
+
+

src/dynamic_graph/sot/romeo/__init__.py

+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph 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 Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+
+from __future__ import print_function
+from dynamic_graph.sot.romeo.romeo import Robot

src/dynamic_graph/sot/romeo/romeo.py.in

+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph 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 Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+
+from __future__ import print_function
+
+import numpy as np
+from dynamic_graph.sot import SE3, R3, SO3
+from dynamic_graph.sot.core import \
+    FeatureGeneric, FeatureJointLimits, Task, Constraint, GainAdaptive, SOT
+from dynamic_graph.sot.dynamics import AngleEstimator
+from dynamic_graph import enableTrace, plug
+from dynamic_graph.sot.se3 import R3, SO3, SE3
+import numpy as np
+
+from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
+from dynamic_graph.sot.dynamics import Dynamic
+
+romeo_pkgdatarootdir = "@ROMEO_PKGDATAROOTDIR@"
+
+class Robot (AbstractHumanoidRobot):
+    """
+    This class instanciates Aldebaran Romeo robot
+    """
+    halfSitting = (
+        
+        0, 0, 0.840252, 0, 0, 0,                         # Free flyer
+        0,                                               # chest
+        0, 0, 0, 0,                                      # head
+        1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2,         # left arm   
+        1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2,          # right arm  
+        0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0,   # left leg   
+        0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, ) # right leg  
+
+#    def smallToFull(self, config):
+#        res = (config + 12*(0.,))
+#        return res
+
+    def __init__(self, name, 
+                 modelDir = romeo_pkgdatarootdir,
+                 xmlDir = romeo_pkgdatarootdir,
+                 device = None,
+                 dynamicType = Dynamic,
+                 tracer = None):
+        AbstractHumanoidRobot.__init__ (self, name, tracer)
+
+        self.OperationalPoints.append('waist')
+        self.OperationalPoints.append('chest')
+        self.device = device
+        self.modelDir = modelDir
+        self.modelName = 'Romeo.wrl'
+        self.specificitiesPath = xmlDir + '/RomeoSpecificities.xml'
+        self.jointRankPath = xmlDir + '/RomeoLinkJointRank.xml'
+
+        self.dynamic = self.loadModelFromJrlDynamics(
+            self.name + '_dynamic',
+            modelDir,
+            self.modelName,
+            self.specificitiesPath,
+            self.jointRankPath,
+            dynamicType)
+
+        self.dimension = self.dynamic.getDimension()
+        print (self.dimension)
+        if self.dimension != len(self.halfSitting):
+            raise RuntimeError("invalid half-sitting pose")
+        self.initializeRobot()
+
+# TODO: ??
+# __all__ = [Romeo]
+