Merge branch 'WBM-Class' into updateRepo

.gitignore

@@ -1,4 +1,11 @@
-##---------------------------------------------------
+# Ignore the build directory
+build/
+
+# OS X Finder files
+.DS_Store
+
+## Matlab: ------------------------------------------
+##
 ## Remove autosaves generated by the Matlab editor
 ## We have git for backups!
 ##---------------------------------------------------
@@ -15,12 +22,17 @@
 # Simulink Code Generation
 slprj/
 
-# Matlab binary .mat files 
+# Matlab binary .mat files
 *.mat
 
 # Matlab figures
-*.fig 
+*.fig
 
-# OS X Finder files
-.DS_Store
+## Sublime Text: ------------------------------------
+##
+## Ignore projects and workspaces.
+## url: https://www.sublimetext.com
+##---------------------------------------------------
+*.sublime-project
+*.sublime-workspace
 

examples/rigidBodyDynamics.m

@@ -11,7 +11,7 @@
 %% The function is searching the file in the current working
 %% directory, so make sure that you are launching the script
 %% in the directory where rigidBody.urdf is found
-wbm_modelInitialiseFromURDF('rigidBody.urdf');
+wbm_modelInitializeFromURDF('rigidBody.urdf');
 
 %% the number of (internal) dofs is 0 for a rigid body
 %% we set the state to some random values, just to show how to
@@ -32,7 +32,7 @@
 M = wbm_massMatrix();
 
 % and the generalized bias forces (coriolis + gravity forces)
-h = wbm_generalisedBiasForces();
+h = wbm_generalizedBiasForces();
 
 % We can check them by printing them:
 disp(M)
@@ -49,4 +49,3 @@
 % and the last 4 elements are the quaternion corresponding to the rotation between the frame and the world frame
 % for more info, check wbm_forwardKinematics options
 disp(world_T_frame)
-

mex-wholebodymodel/library/CMakeLists.txt

@@ -1,42 +1,49 @@
-set(SOURCES src/componentmanager.cpp
-            src/mexmain.cpp
-            src/modelcentroidalmomentum.cpp
+set(SOURCES src/mexmain.cpp
+            src/componentmanager.cpp
+            src/modelstate.cpp
             src/modelcomponent.cpp
+            src/modelcentroidalmomentum.cpp
+            src/modelcoriolisbiasforces.cpp
             src/modeldjdq.cpp
             src/modelforwardkinematics.cpp
-            src/modelgeneralisedbiasforces.cpp
+            src/modelgeneralizedbiasforces.cpp
+            src/modelgetfloatingbasestate.cpp
             src/modelgetstate.cpp
-            src/modelinitialise.cpp
-            src/modelinitialiseurdf.cpp
+            src/modelgravitybiasforces.cpp
+            src/modelinitialize.cpp
+            src/modelinitializeurdf.cpp
+            src/modelinversedynamics.cpp
             src/modeljacobian.cpp
             src/modeljointlimits.cpp
             src/modelmassmatrix.cpp
             src/modelsetworldframe.cpp
-            src/modelstate.cpp
+            src/modeltransformationmatrix.cpp
             src/modelupdatestate.cpp
-            src/modelvisualizetrajectory.cpp
 )
 
 set(HEADERS include/componentmanager.h
+            include/modelstate.h
+            include/modelcomponent.h
             include/mexwholebodymodelsettings.h
             include/modelcentroidalmomentum.h
-            include/modelcomponent.h
+            include/modelcoriolisbiasforces.h
             include/modeldjdq.h
             include/modelforwardkinematics.h
-            include/modelgeneralisedbiasforces.h
+            include/modelgeneralizedbiasforces.h
+            include/modelgetfloatingbasestate.h
             include/modelgetstate.h
-            include/modelinitialise.h
-            include/modelinitialiseurdf.h
+            include/modelgravitybiasforces.h
+            include/modelinitialize.h
+            include/modelinitializeurdf.h
+            include/modelinversedynamics.h
             include/modeljacobian.h
             include/modeljointlimits.h
             include/modelmassmatrix.h
             include/modelsetworldframe.h
-            include/modelstate.h
+            include/modeltransformationmatrix.h
             include/modelupdatestate.h
-            include/modelvisualizetrajectory.h
 )
 
-
 source_group("Source Files" FILES ${SOURCES})
 source_group("Headers Files" FILES ${HEADERS})
 
@@ -46,16 +53,14 @@ include(iCubOptions)
 
 SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")
 
-
 #Include directories
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include
                     ${wholeBodyInterface_INCLUDE_DIRS}
                     ${wholeBodyInterfaceIcub_INCLUDE_DIRS}
                     ${iDynTree_INCLUDE_DIRS}
                     ${paramHelp_INCLUDE_DIRS}
                     ${skinDynLib_INCLUDE_DIRS}
-                    ${ctrlLib_INCLUDE_DIRS}
-                    ${EIGEN3_INCLUDE_DIR})
+                    ${ctrlLib_INCLUDE_DIRS})
 
 include_directories(${Matlab_INCLUDE_DIRS})
 include_directories(${YARP_INCLUDE_DIRS})

mex-wholebodymodel/library/include/componentmanager.h

@@ -2,6 +2,7 @@
  * Copyright (C) 2014 Robotics, Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
  * Authors: Naveen Kuppuswamy
  * email: [email protected]
+ * modified by: Martin Neururer; email: [email protected]; date: June, 2016 & January, 2017
  *
  * The development of this software was supported by the FP7 EU projects
  * CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b))
@@ -20,77 +21,89 @@
 #ifndef COMPONENTNMAINTAINER_H
 #define COMPONENTNMAINTAINER_H
 
-//standard includes
-#include <string.h>
-
-// external headers
+// global includes
 #include <mex.h>
 
+// library includes
+
+// local includes
+
 // forward declarations
-namespace mexWBIComponent{
+namespace mexWBIComponent
+{
   class ModelState;
   class ModelComponent;
-  class ModelJointLimits;
-  class ModelUpdateState;
+  // ------------------------------------
+  // kinematic/dynamic function classes:
+  // ------------------------------------
+  class ModelCentroidalMomentum;
+  class ModelCoriolisBiasForces;
+  class ModelDJdq;
+  class ModelForwardKinematics;
+  class ModelGeneralizedBiasForces;
+  class ModelGetFloatingBaseState;
   class ModelGetState;
-  class ModelGeneralisedBiasForces;
-  class ModelDjDq;
+  class ModelGravityBiasForces;
+  class ModelInitialize;
+  class ModelInitializeURDF;
+  class ModelInverseDynamics;
   class ModelJacobian;
-  class ModelInitialiseURDF;
-  class ModelForwardKinematics;
-  class ModelVisualizeTrajectory;
-  class ModelCentroidalMomentum;
+  class ModelJointLimits;
   class ModelMassMatrix;
   class ModelSetWorldFrame;
-  class ModelInitialise;
+  class ModelTransformationMatrix;
+  class ModelUpdateState;
 }
-// #include "modelsetworldlink.h"
 
 namespace mexWBIComponent
 {
-class ComponentManager
-{
-public:
-
-  static ComponentManager *getInstance(std::string robotName = "icub");
-
-  /**
-   * Delete the (static) instance of this component,
-   * and set the instance pointer to NULL.
-   */
-  static void deleteInstance();
-
-  bool processFunctionCall(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]);
-  void cleanup();
-   ~ComponentManager(void);
-
-  //int getDofs();
-private:
-  ComponentManager(std::string);
-  static ComponentManager *componentManager;
-  void initialise(std::string);
-
-  ModelState *modelState;
-  ModelComponent *currentComponent;
-  ModelJointLimits *modelJointLimits;
-  ModelMassMatrix *modelMassMatrix;
-  ModelUpdateState *modelUpdateState;
-  ModelGetState *modelGetState;
-  ModelGeneralisedBiasForces *modelGeneralisedBiasForces;
-  ModelDjDq *modelDjDq;
-  ModelJacobian *modelJacobian;
-  ModelInitialise *modelInitialise;
-  ModelInitialiseURDF *modelInitialiseURDF;
-  ModelForwardKinematics *modelForwardKinematics;
-  ModelVisualizeTrajectory *modelVisualizeTrajectory;
-  ModelCentroidalMomentum *modelCentroidalMomentum;
-  ModelSetWorldFrame *modelSetWorldFrame;
-
-  int numDof;
-
-  //static wbi::iWholeBodyModel *robotModel;
-  std::map <std::string, ModelComponent*> componentList;
-};
+  class ComponentManager
+  {
+    public:
+      static ComponentManager *getInstance(std::string robotName = "icub");
+
+      bool processFunctionCall(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs);
+
+      /**
+       * Delete the (static) instance of this component,
+       * and set the instance pointer to 0.
+       */
+      static void deleteInstance();
+
+      ~ComponentManager();
+
+    private:
+      ComponentManager(std::string robotName);
+      void initialise(std::string robotName);
+
+      static void cleanup();
+
+      static ComponentManager *componentManager;
+
+      static ModelState     *modelState;
+      static ModelComponent *currentComponent;
+
+      static ModelCentroidalMomentum    *modelCentroidalMomentum;
+      static ModelCoriolisBiasForces    *modelCoriolisBiasForces;
+      static ModelDJdq                  *modelDJdq;
+      static ModelForwardKinematics     *modelForwardKinematics;
+      static ModelGeneralizedBiasForces *modelGeneralizedBiasForces;
+      static ModelGetFloatingBaseState  *modelGetFloatingBaseState;
+      static ModelGetState              *modelGetState;
+      static ModelGravityBiasForces     *modelGravityBiasForces;
+      static ModelInitialize            *modelInitialize;
+      static ModelInitializeURDF        *modelInitializeURDF;
+      static ModelInverseDynamics       *modelInverseDynamics;
+      static ModelJacobian              *modelJacobian;
+      static ModelJointLimits           *modelJointLimits;
+      static ModelMassMatrix            *modelMassMatrix;
+      static ModelSetWorldFrame         *modelSetWorldFrame;
+      static ModelTransformationMatrix  *modelTransformationMatrix;
+      static ModelUpdateState           *modelUpdateState;
+
+      static int numDof;
+      static std::map<std::string, ModelComponent*> componentList;
+  };
 
 }
 

mex-wholebodymodel/library/include/mexwholebodymodelsettings.h

@@ -17,5 +17,4 @@
  * Public License for more details
  */
 
-
 #define MEX_WBMODEL_MAX_NUM_JOINTS 35