Merge pull request #13 from proyan/master

Eigen and Pinocchio with changes in python class Robot

.gitignore

+*~
\ No newline at end of file

.gitmodules

@@ -2,3 +2,6 @@
 	path = cmake
 	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
 
+[submodule ".travis"]
+	path = .travis
+	url = git://github.com/jrl-umi3218/jrl-travis

.travis @ 2a03b5bf

+Subproject commit 2a03b5bf12ff198b7bc97d45c39a531db54c3a58

.travis.yml

+language: cpp
+sudo: required
+compiler: gcc
+env:
+  global:
+  - APT_DEPENDENCIES="doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev
+    libblas-dev gfortran python-dev python-sphinx python-numpy libbullet-dev ros-hydro-angles
+    ros-hydro-common-msgs
+    ros-hydro-control-msgs
+    ros-hydro-realtime-tools
+    ros-hydro-resource-retriever
+    ros-hydro-robot-state-publisher
+    ros-hydro-ros-base
+    ros-hydro-tf
+    ros-hydro-urdf
+    ros-hydro-urdfdom
+    ros-hydro-urdfdom-py"
+  - GIT_DEPENDENCIES="stack-of-tasks/pinocchio:devel
+    proyan/dynamic-graph proyan/dynamic-graph-python proyan/sot-core
+    proyan/sot-tools proyan/sot-dynamic:topic/sot-pinocchio "
+  - ROS_DISTRO=hydro
+  - ROS_GIT_DEPENDENCIES="proyan/dynamic_graph_bridge_msgs proyan/dynamic_graph_bridge"
+  - ALLOW_CATKINLINT_FAILURE=true
+notifications:
+  email:
+  - francois.keith@gmail.com
+branches:
+  only:
+  - master
+script: ./.travis/run build
+after_success: ./.travis/run after_success
+after_failure: ./.travis/run after_failure
+before_install: ./.travis/run dependencies/catkin; ./.travis/run before_install
+

CMakeLists.txt

@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/eigen.cmake)
 INCLUDE(cmake/lapack.cmake)
 INCLUDE(cmake/cpack.cmake)
 INCLUDE(cmake/ros.cmake)
@@ -38,30 +39,29 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
 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_OPTIONAL_DEPENDENCY("hrp2-dynamics >= 1.5.0")
+ADD_REQUIRED_DEPENDENCY("pinocchio")
 ADD_OPTIONAL_DEPENDENCY("hrp2-10-optimized >= 1.0.1")
 
 ADD_OPTIONAL_DEPENDENCY("hrp2-10 >= 1.0.1-10")
 ADD_OPTIONAL_DEPENDENCY("hrp2-14 >= 1.8-6")
 
-ADD_REQUIRED_DEPENDENCY("dynamic-graph")
-ADD_REQUIRED_DEPENDENCY("sot-core")
-ADD_REQUIRED_DEPENDENCY("sot-dynamic >= 2.8")
-ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-dynamic >= 3.1")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
 
 ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge_msgs")
-ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge")
+ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge >= 3.0")
 # Search for dependencies.
 # Boost
 SET(BOOST_COMPONENTS filesystem system thread)
 SEARCH_FOR_BOOST()
+SEARCH_FOR_EIGEN()
 
 # Handle rpath necessary to handle ROS multiplace packages
 # libraries inclusion
+SET(CMAKE_INSTALL_LIBDIR lib)
 SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
 SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
 SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")

cmake @ c8ae5972

-Subproject commit fa2738146fa19d8bbbb1d8c8a97a2b4809ce81af
+Subproject commit c8ae5972e825bc27bf978f142b81915df19fef70

src/CMakeLists.txt

@@ -24,10 +24,9 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
     SOVERSION ${PROJECT_VERSION})
 
   PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph)
-  PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-mathtools)
-  PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-mal)
+  PKG_CONFIG_USE_DEPENDENCY(${NAME} eigen3)
   PKG_CONFIG_USE_DEPENDENCY(${NAME} sot-core)
-  PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-dynamics)
+  PKG_CONFIG_USE_DEPENDENCY(${NAME} pinocchio)
 
   INSTALL(TARGETS ${NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
 
@@ -40,27 +39,6 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
     )
 ENDFUNCTION()
 
-# Compile plug-ins.
-IF (HRP2_DYNAMICS_FOUND)
-  MESSAGE(STATUS "Add dynamic-hrp2_14 plugin")
-  COMPILE_PLUGIN(dynamic-hrp2_14 dynamic-hrp2_14.cc DynamicHrp2_14)
-  PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_14 hrp2-dynamics)
-ENDIF()
-
-IF (HRP2_10_OPTIMIZED_FOUND)
-  MESSAGE(STATUS "Add dynamic-hrp2_10 plugin")
-  COMPILE_PLUGIN(dynamic-hrp2_10 dynamic-hrp2_10.cc DynamicHrp2_10)
-  PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_10 hrp2-10-optimized)
-ENDIF()
-
-IF (HRP2_10_OPTIMIZED_FOUND)
-  CONFIG_FILES(dynamic_graph/sot/hrp2_10/robot.py)
-ENDIF()
-
-IF (HRP2_DYNAMICS_FOUND)
-  CONFIG_FILES(dynamic_graph/sot/hrp2_14/robot.py)
-ENDIF()
-
 # Install Python files.
 SET(PYTHON_MODULE_DIR
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/hrp2)
@@ -70,23 +48,20 @@ SET(PYTHON_MODULE_BUILD_DIR
 SET(PYTHON_MODULE dynamic_graph/sot/hrp2)
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "hrp2.py" )
+
 SET(FILES __init__.py robot.py)
 
 # Install dynamic_graph.sot.hrp2_14
-IF (HRP2_DYNAMICS_FOUND)
-  SET(PYTHON_MODULE dynamic_graph/sot/hrp2_14)
-  PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
-  PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
-  PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
-ENDIF()
+SET(PYTHON_MODULE dynamic_graph/sot/hrp2_14)
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
 
 # Install dynamic_graph.sot.hrp2_10
-IF (HRP2_10_OPTIMIZED_FOUND)
-  SET(PYTHON_MODULE dynamic_graph/sot/hrp2_10)
-  PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
-  PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
-  PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
-ENDIF()
+SET(PYTHON_MODULE dynamic_graph/sot/hrp2_10)
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
 
 # Add the library to wrap the device of HRP2.
 MACRO(build_hrp2_device)
@@ -152,11 +127,15 @@ MACRO(build_hrp2_controller robotnumber)
 
 ENDMACRO()
 
-IF (HRP2_10_OPTIMIZED_FOUND)
-  build_hrp2_controller("10")
-ENDIF()
+#TODO: which? 10 or 14?
+build_hrp2_controller("14")
+build_hrp2_controller("10")
+
+#IF (HRP2_10_OPTIMIZED_FOUND)
+#  build_hrp2_controller("10")
+#ENDIF()
 
-IF (HRP2_DYNAMICS_FOUND)
-  build_hrp2_controller("14")
-ENDIF()
+#IF (HRP2_DYNAMICS_FOUND)
+#  build_hrp2_controller("14")
+#ENDIF()
 

src/dynamic-hrp2_10.cc

@@ -32,7 +32,7 @@ namespace dynamicgraph
     namespace hrp2
     {
       DynamicHrp2_10::DynamicHrp2_10 (const std::string & name)
-	: Dynamic (name, false)
+	: Dynamic (name)
       {
 	sotDEBUGIN(15);
 	DynamicHrp2_10::buildModelHrp2 ();

src/dynamic_graph/sot/hrp2/hrp2.py

@@ -17,15 +17,13 @@
 from __future__ import print_function
 
 import numpy as np
-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.dynamics.dynamic_hrp2 import DynamicHrp2
-# from dynamic_graph.sot.dynamics.dynamic_hrp2_10 import DynamicHrp2_10
 
+#Don't change this order
 from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
+from dynamic_graph.ros import RosRobotModel
+import pinocchio as se3
+from rospkg import RosPack
+
 
 # Internal helper tool.
 def matrixToTuple(M):
@@ -67,18 +65,15 @@ class Hrp2(AbstractHumanoidRobot):
         res = (config + 10*(0.,))
         return res
 
-    def __init__(self, name, modelDir, xmlDir, device, dynamicType,
-                 tracer = None):
+    def __init__(self, name, robotnumber,
+                 device = None, tracer = None):
+        
         AbstractHumanoidRobot.__init__ (self, name, tracer)
 
         self.OperationalPoints.append('waist')
         self.OperationalPoints.append('chest')
         self.device = device
-        self.modelDir = modelDir
-        self.modelName = 'HRP2JRLmainsmall.wrl'
-        self.specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
-        self.jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
-
+        
         self.AdditionalFrames.append(
             ("accelerometer",
              matrixToTuple(self.accelerometerPosition), "chest"))
@@ -90,22 +85,30 @@ class Hrp2(AbstractHumanoidRobot):
              self.forceSensorInLeftAnkle, "left-ankle"))
         self.AdditionalFrames.append(
             ("rightFootForceSensor",
-            self.forceSensorInRightAnkle, "right-ankle"))
+             self.forceSensorInRightAnkle, "right-ankle"))
+        self.OperationalPointsMap = {'left-wrist'  : 'LARM_JOINT5',
+                                     'right-wrist' : 'RARM_JOINT5',
+                                     'left-ankle'  : 'LLEG_JOINT5',
+                                     'right-ankle' : 'RLEG_JOINT5',
+                                     'gaze'        : 'HEAD_JOINT1',
+                                     'waist'       : 'WAIST',
+                                     'chest'       : 'CHEST_JOINT1'}
 
-        self.dynamic = self.loadModelFromJrlDynamics(
-            self.name + '_dynamic',
-            modelDir,
-            self.modelName,
-            self.specificitiesPath,
-            self.jointRankPath,
-            dynamicType)
+        self.dynamic = RosRobotModel("{0}_dynamic".format(name))
 
-        self.dimension = self.dynamic.getDimension()
         
-        self.plugVelocityFromDevice = True
 
+        rospack = RosPack()
+        self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}.urdf'.format(robotnumber)
+
+        self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer())
+        self.pinocchioData = self.pinocchioModel.createData()
+        self.dynamic.setModel(self.pinocchioModel)
+        self.dynamic.setData(self.pinocchioData)
+        self.dimension = self.dynamic.getDimension()
+        self.plugVelocityFromDevice = True
         if self.dimension != len(self.halfSitting):
             raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension))
         self.initializeRobot()
-
+        self.dynamic.displayModel()
 __all__ = [Hrp2]

src/dynamic_graph/sot/hrp2_10/robot.py.in → src/dynamic_graph/sot/hrp2_10/robot.py

@@ -17,8 +17,7 @@
 from __future__ import print_function
 from dynamic_graph.sot.hrp2 import Hrp2
 import numpy as np
-from dynamic_graph.sot.hrp2.dynamic_hrp2_10 import DynamicHrp2_10
-hrp2_10_pkgdatarootdir = "@HRP2_10_PKGDATAROOTDIR@/hrp2-10"
+
 
 # Internal helper tool.
 def matrixToTuple(M):
@@ -49,8 +48,6 @@ class Robot (Hrp2):
         )
 
     def __init__(self, name,
-                 modelDir = hrp2_10_pkgdatarootdir,
-                 xmlDir = hrp2_10_pkgdatarootdir,
                  device = None,
                  tracer = None):
 
@@ -122,7 +119,6 @@ class Robot (Hrp2):
                 ("cameraXtionRGB",
                  matrixToTuple(cameraXtionRGBPosition), "gaze"))
 
-        Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
-                      tracer)
+        Hrp2.__init__(self, name, "10", device, tracer)
 
 __all__ = ["Robot"]

src/dynamic_graph/sot/hrp2_14/robot.py.in → src/dynamic_graph/sot/hrp2_14/robot.py

@@ -18,10 +18,6 @@ from __future__ import print_function
 from dynamic_graph.sot.hrp2 import Hrp2
 import numpy as np
 
-from dynamic_graph.sot.hrp2.dynamic_hrp2_14 import DynamicHrp2_14
-
-hrp2_14_pkgdatarootdir = "@HRP2_14_PKGDATAROOTDIR@/hrp2-14"
-
 # Internal helper tool.
 def matrixToTuple(M):
     tmp = M.tolist()
@@ -38,21 +34,26 @@ class Robot (Hrp2):
         # Free flyer
         0., 0., 0.648702, 0., 0. , 0.,
 
-        # Legs
-        0., 0., -0.453786, 0.872665, -0.418879, 0.,
-        0., 0., -0.453786, 0.872665, -0.418879, 0.,
-
         # Chest and head
         0., 0., 0., 0.,
 
-        # Arms
+        # Left Arm
         0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1,
+        # Left hand
+        #0.,0.,0.,0.,0.,
+        
+        #Right Arm
         0.261799, 0.17453,  0., -0.523599, 0., 0., 0.1,
+        #Right Hand
+        #0.,0.,0.,0.,0.,
+
+        # Legs
+        0., 0., -0.453786, 0.872665, -0.418879, 0.,
+        0., 0., -0.453786, 0.872665, -0.418879, 0.,
+
         )
 
     def __init__(self, name,
-                 modelDir = hrp2_14_pkgdatarootdir,
-                 xmlDir = hrp2_14_pkgdatarootdir,
                  device = None,
                  tracer = None):
 
@@ -112,6 +113,6 @@ class Robot (Hrp2):
                 ("cameraTopRight",
                  matrixToTuple(cameraTopRightPosition), "gaze"))
 
-        Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_14, tracer)
+        Hrp2.__init__(self, name, "14", device, tracer)
 
 __all__ = ["Robot"]

src/sot-hrp2-controller.cpp

@@ -50,7 +50,7 @@ SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
   device_(RobotName)
 {
 
-  std::cout << "Going through SoTHRP2Controller." << std::endl;
+  std::cout << "Going through SoTHRP2Controller for " <<RobotName<< std::endl;
   boost::thread thr(workThread,this);
   sotDEBUG(25) << __FILE__ << ":" 
 	       << __FUNCTION__ <<"(#" 

src/sot-hrp2-device.cpp

@@ -52,10 +52,10 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
   for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
   signalRegistration (robotState_ << accelerometerSOUT_ << gyrometerSOUT_
                       << currentSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
-  ml::Vector data (3); data.setZero ();
+  dg::Vector data (3); data.setZero ();
   accelerometerSOUT_.setConstant (data);
   gyrometerSOUT_.setConstant (data);
-  baseff_.resize(12);
+  baseff_.resize(7);
 
   using namespace dynamicgraph::command;
   std::string docstring;
@@ -229,13 +229,13 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
   int time = controlSIN.getTime ();
   zmpSIN.recompute (time + 1);
   // Express ZMP in free flyer reference frame
-  ml::Vector zmpGlobal (4);
+  dg::Vector zmpGlobal (4);
   for (unsigned int i = 0; i < 3; ++i)
     zmpGlobal(i) = zmpSIN(time + 1)(i);
   zmpGlobal(3) = 1.;
   dgsot::MatrixHomogeneous inversePose;
-  freeFlyerPose().inverse(inversePose);
-  ml::Vector localZmp = inversePose * zmpGlobal;
+  inversePose = freeFlyerPose().inverse(Eigen::Affine);
+  dg::Vector localZmp(4); localZmp = inversePose.matrix() * zmpGlobal;
   vector<double> ZMPRef(3);
   for(unsigned int i=0;i<3;++i)
     ZMPRef[i] = localZmp(i);
@@ -244,11 +244,17 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
   controlOut["zmp"].setValues(ZMPRef);
 
   // Update position of freeflyer in global frame
-  for (int i = 0;i < 3; ++i)
-    baseff_[i*4+3] = freeFlyerPose () (i, 3);
-  for(unsigned i = 0;i < 3; ++i)
-    for(unsigned j = 0; j < 3; ++j)
-      baseff_[i * 4 + j] = freeFlyerPose () (i, j);
+  Eigen::Vector3d transq_(freeFlyerPose().translation());
+  dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
+
+  //translation
+  for(int i=0; i<3; i++) baseff_[i] = transq_(i);
+  
+  //rotation: quaternion
+  baseff_[3] = qt_.w();
+  baseff_[4] = qt_.x();
+  baseff_[5] = qt_.y();
+  baseff_[6] = qt_.z();
 
   controlOut["baseff"].setValues(baseff_);
   sotDEBUGOUT(25) ;

src/sot-hrp2-device.hh

@@ -24,9 +24,10 @@
 #include <dynamic-graph/linear-algebra.h>
 #include <sot/core/device.hh>
 #include <sot/core/abstract-sot-external-interface.hh>
-#include <sot/core/matrix-rotation.hh>
+#include <sot/core/matrix-geometry.hh>
 
 namespace dgsot=dynamicgraph::sot;
+namespace dg=dynamicgraph;
 
 class SoTHRP2Device: public 
     dgsot::Device
@@ -60,7 +61,7 @@ protected:
   double timestep_;
   
   /// \brief Previous robot configuration.
-  maal::boost::Vector previousState_;
+  dg::Vector previousState_;
   
   /// \brief Robot state provided by OpenHRP.
   ///
@@ -68,29 +69,29 @@ protected:
   /// account the stabilization step. Therefore, this usually
   /// does *not* match the state control input signal.
   ///
-  dynamicgraph::Signal<ml::Vector, int> robotState_;
+  dynamicgraph::Signal<dg::Vector, int> robotState_;
 
   /// Accelerations measured by accelerometers
-  dynamicgraph::Signal <ml::Vector, int> accelerometerSOUT_;
+  dynamicgraph::Signal <dg::Vector, int> accelerometerSOUT_;
   /// Rotation velocity measured by gyrometers
-  dynamicgraph::Signal <ml::Vector, int> gyrometerSOUT_;
+  dynamicgraph::Signal <dg::Vector, int> gyrometerSOUT_;
   /// motor currents
-  dynamicgraph::Signal <ml::Vector, int> currentSOUT_;
+  dynamicgraph::Signal <dg::Vector, int> currentSOUT_;
 
   /// proportional and derivative position-control gains
-  dynamicgraph::Signal <ml::Vector, int> p_gainsSOUT_;
-  dynamicgraph::Signal <ml::Vector, int> d_gainsSOUT_;
+  dynamicgraph::Signal <dg::Vector, int> p_gainsSOUT_;
+  dynamicgraph::Signal <dg::Vector, int> d_gainsSOUT_;
 
   /// Intermediate variables to avoid allocation during control
-  ml::Vector mlforces;
-  ml::Vector mlRobotState;
+  dg::Vector mlforces;
+  dg::Vector mlRobotState;
   dgsot::MatrixRotation pose;
-  ml::Vector accelerometer_;
-  ml::Vector gyrometer_;
+  dg::Vector accelerometer_;
+  dg::Vector gyrometer_;
   std::vector<double> baseff_;
-  ml::Vector torques_;
-  ml::Vector currents_;
-  ml::Vector p_gains_;
-  ml::Vector d_gains_;
+  dg::Vector torques_;
+  dg::Vector currents_;
+  dg::Vector p_gains_;
+  dg::Vector d_gains_;
 };
 #endif /* _SOT_HRP2Device_H_*/