Merge pull request #526 from stack-of-tasks/devel

Merge devel into master

.gitlab-ci.yml

@@ -1,6 +1,8 @@
+# Please don't edit this file, and use the version generated at
+# http://rainboard.laas.fr/project/pinocchio/.gitlab-ci.yml
+
 variables:
   GIT_SUBMODULE_STRATEGY: "recursive"
-  GIT_DEPTH: "3"
   CCACHE_BASEDIR: "${CI_PROJECT_DIR}"
   CCACHE_DIR: "${CI_PROJECT_DIR}/ccache"
 
@@ -13,12 +15,12 @@ cache:
     - gh-pages
   script:
     - mkdir -p ccache
-    - cd /root/robotpkg/math/pinocchio
+    - cd /root/robotpkg/math
     - git pull
+    - cd pinocchio
     - make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
     - make install
     - cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
-    - make build_tests
     - make test
 
 robotpkg-pinocchio-14.04-release:
@@ -38,35 +40,43 @@ robotpkg-pinocchio-18.04-release:
     - gh-pages
   script:
     - mkdir -p ccache
-    - cd /root/robotpkg/math/py-pinocchio
+    - cd /root/robotpkg/math
     - git pull
+    - cd pinocchio
+    - make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
+    - cd ..
+    - cd py-pinocchio
     - make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
     - make install
     - cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
+    - make test
 
-robotpkg-py-pinocchio-py3-18.04-release:
-  <<: *robotpkg-py-pinocchio
-  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:18.04
-
-robotpkg-py-pinocchio-16.04-release:
-  <<: *robotpkg-py-pinocchio
-  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:16.04
-
-robotpkg-py-pinocchio-18.04-release:
+robotpkg-py-pinocchio-14.04-release:
   <<: *robotpkg-py-pinocchio
-  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:18.04
+  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:14.04
 
 robotpkg-py-pinocchio-py3-14.04-release:
   <<: *robotpkg-py-pinocchio
   image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:14.04
+  allow_failure: true
 
-robotpkg-py-pinocchio-14.04-release:
+robotpkg-py-pinocchio-16.04-release:
   <<: *robotpkg-py-pinocchio
-  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:14.04
+  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:16.04
 
 robotpkg-py-pinocchio-py3-16.04-release:
   <<: *robotpkg-py-pinocchio
   image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:16.04
+  allow_failure: true
+
+robotpkg-py-pinocchio-18.04-release:
+  <<: *robotpkg-py-pinocchio
+  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio:18.04
+
+robotpkg-py-pinocchio-py3-18.04-release:
+  <<: *robotpkg-py-pinocchio
+  image: eur0c.laas.fr:5000/stack-of-tasks/pinocchio/py-pinocchio-py3:18.04
+  allow_failure: true
 
 doc-coverage:
   <<: *robotpkg-py-pinocchio
@@ -86,4 +96,3 @@ doc-coverage:
     paths:
       - doxygen-html/
       - coverage/
-

README.md

@@ -6,10 +6,10 @@ Pinocchio: a C++ library for efficient Rigid Multi-body Dynamics computations
 [![Coverage Status](https://coveralls.io/repos/github/stack-of-tasks/pinocchio/badge.svg?branch=devel)](https://coveralls.io/github/stack-of-tasks/pinocchio?branch=devel)
 [![Coverity Scan Build Status](https://scan.coverity.com/projects/7824/badge.svg)](https://scan.coverity.com/projects/pinocchio)
 
-**Pinocchio** instatiates state-of-the-art Rigid Body Algotithms for poly-articulated systems based on revisited Roy Featherstone's algorithms.
+**Pinocchio** instantiates state-of-the-art Rigid Body Algorithms for poly-articulated systems based on revisited Roy Featherstone's algorithms.
 In addition, **Pinocchio** instantiates analytical derivatives of the main Rigid-Body Algorithms like the Recursive Newton-Euler Algorithms or the Articulated-Body Algorithm.
-**Pinocchio** is first tailored for legged robotics applications, but it can be used in extra contextes.
-It is built upon Eigen for linear algebra and FCL for collision detections. **Pinocchio** comes with a Python interface for fast code protyping.
+**Pinocchio** is first tailored for legged robotics applications, but it can be used in extra contexts.
+It is built upon Eigen for linear algebra and FCL for collision detection. **Pinocchio** comes with a Python interface for fast code prototyping.
 
 **Pinocchio** is now at the hearth of various robotics softwares as the [Stack-of-Tasks](http://stack-of-tasks.github.io) or the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc).
 
@@ -27,7 +27,7 @@ We provide some basic examples on how to use **Pinocchio** in Python in the [exa
 
 ## Tutorials 
 
-**Pinocchio** is comming with a large bunch of tutorials aiming at introducting the basic tools for robotics control.
+**Pinocchio** is coming with a large bunch of tutorials aiming at introducing the basic tools for robotics control.
 The content of the tutorials are described [here](http://projects.laas.fr/gepetto/index.php/Teach/Supaero2018) and the source code of these tutorials is located [here](https://github.com/stack-of-tasks/pinocchio-tutorials).
 
 ## Dependencies
@@ -47,7 +47,7 @@ The content of the tutorials are described [here](http://projects.laas.fr/gepett
 ### Optional dependencies
    - urdfdom (version >= 0.2)
    - LUA 5.1
-   - [FCL](https://github.com/flexible-collision-library/fcl)
+   - [HPP-FCL](https://github.com/humanoid-path-planner/hpp-fcl)
 
 ### Python bindings
    - Python 2.7 or 3.0
@@ -96,4 +96,4 @@ If you have taken part to the development of **Pinocchio**, feel free to add you
 
 ## Acknowledgments
 
-The development of **Pinocchio** is supported by the [Gepetto team](http://projects.laas.fr/gepetto/) @LAAS-CNRS.
+The development of **Pinocchio** is supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr) and the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr).

bindings/python/algorithm/expose-frames.cpp

@@ -45,20 +45,88 @@ namespace se3
                                                )
     {
       computeJointJacobians(model,data,q);
-      framesForwardKinematics(model,data);
+      updateFramePlacements(model,data);
 
       return get_frame_jacobian_proxy(model, data, frame_id, rf);
     }
+
+
+    static Data::Matrix6x
+    get_frame_jacobian_time_variation_proxy(const Model & model,
+                                            Data & data,
+                                            Model::FrameIndex jointId,
+                                            ReferenceFrame rf)
+    {
+      Data::Matrix6x dJ(6,model.nv); dJ.setZero();
+
+      if(rf == LOCAL) getFrameJacobianTimeVariation<LOCAL>(model,data,jointId,dJ);
+      else getFrameJacobianTimeVariation<WORLD>(model,data,jointId,dJ);
+
+      return dJ;
+    }
+
+    static Data::Matrix6x frame_jacobian_time_variation_proxy(const Model & model,
+                                                              Data & data,
+                                                              const Eigen::VectorXd & q,
+                                                              const Eigen::VectorXd & v,
+                                                              const Model::FrameIndex frame_id,
+                                                              ReferenceFrame rf
+                                                              )
+    {
+      computeJointJacobiansTimeVariation(model,data,q,v);
+      updateFramePlacements(model,data);
+
+      return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
+    }        
+
+    static Motion get_frame_velocity_proxy(const Model & model,
+                                           Data & data,
+                                           const Model::FrameIndex frame_id
+                                           )
+    {
+      Motion v;
+      getFrameVelocity(model,data,frame_id,v);
+      return v;
+    }
+
+    static Motion get_frame_acceleration_proxy(const Model & model,
+                                               Data & data,
+                                               const Model::FrameIndex frame_id
+                                               )
+    {
+      Motion a;
+      getFrameAcceleration(model,data,frame_id,a);
+      return a;
+    }
 
     void exposeFramesAlgo()
     {
-      bp::def("framesKinematics",
-              (void (*)(const Model &, Data &))&framesForwardKinematics,
+      bp::def("updateFramePlacements",
+              (void (*)(const Model &, Data &))&updateFramePlacements,
               bp::args("Model","Data"),
               "Computes the placements of all the operational frames according to the current joint placement stored in data"
-              "and put the results in data.");
+              "and puts the results in data.");
+
+      bp::def("updateFramePlacement",
+              (const SE3 & (*)(const Model &, Data &, const Model::FrameIndex))&updateFramePlacement,
+              bp::args("Model","Data","Operational frame ID (int)"),
+              "Computes the placement of the given operational frames according to the current joint placement stored in data,"
+              "puts the results in data and returns it.",
+              bp::return_value_policy<bp::return_by_value>());
+
+      bp::def("getFrameVelocity",
+              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_velocity_proxy,
+              bp::args("Model","Data","Operational frame ID (int)"),
+              "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
+              "Fist or second order forwardKinematics should be called first.");
+
+      bp::def("getFrameAcceleration",
+              (Motion (*)(const Model &, Data &, const Model::FrameIndex))&get_frame_acceleration_proxy,
+              bp::args("Model","Data","Operational frame ID (int)"),
+              "Returns the spatial velocity of the frame expressed in the LOCAL frame coordinate system."
+              "Second order forwardKinematics should be called first.");
 
-      bp::def("framesKinematics",
+      bp::def("framesForwardKinematics",
               (void (*)(const Model &, Data &, const Eigen::VectorXd &))&framesForwardKinematics,
               bp::args("Model","Data",
                        "Configuration q (size Model::nq)"),
@@ -87,7 +155,28 @@ namespace se3
               "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
               "where v is the time derivative of the configuration q.\n"
               "Be aware that computeJointJacobians and framesKinematics must have been called first.");
-
+
+      bp::def("frameJacobianTimeVariation",
+              (Data::Matrix6x (*)(const Model &, Data &, const Eigen::VectorXd &,const Eigen::VectorXd &, const Model::FrameIndex, ReferenceFrame))&frame_jacobian_time_variation_proxy,
+              bp::args("Model","Data",
+                       "Configuration q (size Model::nq)",
+                       "Joint velocity v (size Model::nv)",                       
+                       "Operational frame ID (int)",
+                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
+              "Computes the Jacobian Time Variation of the frame given by its ID either in the local or the world frames."
+              "The columns of the Jacobian time variation are expressed in the frame coordinates.\n"
+              "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
+              "where v is the time derivative of the configuration q.");      
+
+      bp::def("getFrameJacobianTimeVariation",get_frame_jacobian_time_variation_proxy,
+              bp::args("Model, the model of the kinematic tree",
+                       "Data, the data associated to the model where the results are stored",
+                       "Frame ID, the index of the frame.",
+                       "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
+              "Returns the Jacobian time variation of a specific frame (specified by Frame ID) expressed either in the world or the local frame."
+              "You have to call computeJointJacobiansTimeVariation and framesKinematics first."
+              "If rf is set to LOCAL, it returns the jacobian time variation associated to the frame index. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");
+
     }
   } // namespace python
 } // namespace se3

bindings/python/algorithm/expose-jacobian.cpp

@@ -78,7 +78,7 @@ namespace se3
               "Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
               "The result is accessible through data.J. This function computes also the forwardKinematics of the model.",
               bp::return_value_policy<bp::return_by_value>());
-      
+
       bp::def("computeJointJacobians",(const Data::Matrix6x &(*)(const Model &, Data &))&computeJointJacobians,
               bp::args("Model","Data"),
               "Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame.\n"
@@ -94,7 +94,7 @@ namespace se3
                        "update_kinematics (true = update the value of the total jacobian)"),
               "Computes the jacobian of a given given joint according to the given input configuration."
               "If rf is set to LOCAL, it returns the jacobian associated to the joint frame. Otherwise, it returns the jacobian of the frame coinciding with the world frame.");
-      
+
       bp::def("getJointJacobian",get_jacobian_proxy,
               bp::args("Model, the model of the kinematic tree",
                        "Data, the data associated to the model where the results are stored",
@@ -116,7 +116,7 @@ namespace se3
                        "Data, the data associated to the model where the results are stored",
                        "Joint ID, the index of the joint.",
                        "Reference frame rf (either ReferenceFrame.LOCAL or ReferenceFrame.WORLD)"),
-              "Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint."
+              "Computes the Jacobian time variation of a specific joint expressed either in the world frame or in the local frame of the joint."
               "You have to call computeJointJacobiansTimeVariation first."
               "If rf is set to LOCAL, it returns the jacobian time variation associated to the joint frame. Otherwise, it returns the jacobian time variation of the frame coinciding with the world frame.");
     }

bindings/python/parsers/python/model.cpp

@@ -20,17 +20,23 @@
 #include <iostream>
 #include <Python.h>
 #include <boost/shared_ptr.hpp>
+#include <boost/version.hpp>
+
+// Boost 1.58
+#if BOOST_VERSION / 100 % 1000 == 58
+#include <fstream>
+#endif
 
 namespace se3
 {
   namespace python
   {
     namespace bp = boost::python;
-    
+
     Model buildModel(const std::string & filename, const std::string & model_name, bool verbose) throw (bp::error_already_set)
     {
       Py_Initialize();
-      
+
       bp::object main_module = bp::import("__main__");
       // Get a dict for the global namespace to exec further python code with
       bp::dict globals = bp::extract<bp::dict>(main_module.attr("__dict__"));
@@ -42,13 +48,23 @@ namespace se3
       // can update as you want.
       try
       {
+// Boost 1.58
+#if BOOST_VERSION / 100 % 1000 == 58
+        // Avoid a segv with exec_file
+        // See: https://github.com/boostorg/python/pull/15
+        std::ifstream t(filename.c_str());
+        std::stringstream buffer;
+        buffer << t.rdbuf();
+        bp::exec(buffer.str().c_str(), globals);
+#else // default implementation
         bp::exec_file((bp::str)filename, globals);
+#endif
       }
       catch (bp::error_already_set & e)
       {
         PyErr_PrintEx(0);
       }
-      
+
       Model model;
       try
       {
@@ -64,10 +80,10 @@ namespace se3
         std::cout << "Your model has been built. It has " << model.nv;
         std::cout << " degrees of freedom." << std::endl;
       }
-      
+
       // close interpreter
       Py_Finalize();
-        
+
       return model;
     }
   } // namespace python

bindings/python/scripts/deprecated.py

@@ -21,7 +21,15 @@
 from . import libpinocchio_pywrap as se3 
 from .deprecation import deprecated
 
-@deprecated("This function has been renamed computeJointJacobians and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJacobians.")
+
+@deprecated("This function has been renamed updateFramePlacements when taking two arguments, and framesForwardKinematics when taking three. Please change your code to the appropriate method.")
+def framesKinematics(model,data,q=None):
+  if q is None:
+    se3.updateFramePlacements(model,data)
+  else:
+    se3.framesForwardKinematics(model,data,q)
+
+@deprecated("This function has been renamed computeJointJacobians and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJointJacobians.")
 def computeJacobians(model,data,q=None):
   if q is None:
     return se3.computeJointJacobians(model,data)
@@ -42,7 +50,7 @@ def getJacobian(model,data,jointId,local):
   else:
     return se3.getJointJacobian(model,data,jointId,se3.ReferenceFrame.WORLD)
 
-@deprecated("This function has been renamed computeJacobiansTimeVariation and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJacobiansTimeVariation.")
+@deprecated("This function has been renamed computeJacobiansTimeVariation and will be removed in release 1.4.0 of Pinocchio. Please change for new computeJointJacobiansTimeVariation.")
 def computeJacobiansTimeVariation(model,data,q,v):
   return se3.computeJointJacobiansTimeVariation(model,data,q,v)
 

bindings/python/scripts/deprecation.py

@@ -30,6 +30,8 @@ def wrapper(*args, **kwargs):
 
             return func(*args, **kwargs)
 
+        if wrapper.__doc__ is None:
+            wrapper.__doc__ = instructions
         return wrapper
 
     return decorator