...

Commits (98)
 ... ... @@ -3,3 +3,4 @@ Xcode* *~ *.pyc coverage* .vscode*
 ... ... @@ -39,7 +39,7 @@ allow_failures: - compiler: before_install: ./travis_custom/custom_before_install install: - pip install --user coveralls - pip install --user pyopenssl - pip install --user numpy script: - export CMAKE_ADDITIONAL_OPTIONS="-DCMAKE_BUILD_TYPE=${BUILDTYPE} -DBUILD_WITH_COLLISION_SUPPORT=${BUILD_WITH_COLLISION_SUPPORT}" ... ... @@ -51,6 +51,3 @@ after_success: #- export PYTHONPATH=$install_dir/lib/python2.7/site-packages #- coveralls-lcov -v -n$build_dir/coverage.info > coverage.json - export PYTHONPATH=$PYHTONPATH:/tmp/_ci/install/lib/python2.7/site-packages:/usr/lib/python2.7/dist-packages - coveralls-lcov -v -n /tmp/_ci/build/coverage.info > coverage.json - coverage2 run ./python/tests.py - coveralls --merge=coverage.json  ... ... @@ -36,7 +36,7 @@ If you want to learn more on **Pinocchio** internal behaviors and main features, **Pinocchio** is multi-thread friendly. **Pinocchio** is reliable and extensively tested (unit-tests, simulations and real robotics applications). **Pinocchio** is supported on Mac Os X and many Linux distributions ([See build status here](http://robotpkg.openrobots.org/rbulk/robotpkg/math/pinocchio/index.html)). **Pinocchio** is supported on Mac Os X and many Linux distributions ([see build status here](http://robotpkg.openrobots.org/rbulk/robotpkg/math/pinocchio/index.html)). ## Ongoing developments ... ... @@ -48,9 +48,14 @@ If you want to follow the current developments, you can directly refer to the [d **Pinocchio** is also being deployed on ROS, you may follow its deployment status on [Melodic](https://index.ros.org/r/pinocchio/#melodic) or [Kinetic](https://index.ros.org/r/pinocchio/#kinetic). ## Documentation The online **Pinocchio** documentation of the last release is available [here](https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/). ## Examples We provide some basic examples on how to use **Pinocchio** in Python in the [examples/python](./examples/python) directory. Additional examples introducing **Pinocchio** are also available in the [documentation](https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_d-practical-exercises_intro.html) ## Tutorials ... ... @@ -101,9 +106,10 @@ The following people have been involved in the development of **Pinocchio**: - [Guilhem Saurel](http://projects.laas.fr/gepetto/index.php/Members/GuilhemSaurel) (LAAS-CNRS): continuous integration and deployment - [Joseph Mirabel](http://jmirabel.github.io/) (LAAS-CNRS): Lie groups support - [Antonio El Khoury](https://www.linkedin.com/in/antonioelkhoury) (Wandercraft): bug fixes - [Gabriele Buondono](http://projects.laas.fr/gepetto/index.php/Members/GabrieleBuondonno) (LAAS-CNRS): bug fixes - [Gabriele Buondono](http://projects.laas.fr/gepetto/index.php/Members/GabrieleBuondonno) (LAAS-CNRS): features extension, bug fixes and Python bindings - [Florian Valenza](https://fr.linkedin.com/in/florian-valenza-1b274082) (Astek): core developments and FCL support - [Wolfgang Merkt](http://www.wolfgangmerkt.com/) (University of Edinburgh): ROS integration and support - [Rohan Budhiraja](https://scholar.google.com/citations?user=NW9Io9AAAAAJ) (LAAS-CNRS): features extension If you have taken part to the development of **Pinocchio**, feel free to add your name and contribution here. ... ...  ... ... @@ -168,6 +168,21 @@ FOREACH(python${PYTHON_FILES}) DESTINATION ${${PYWRAP}_INSTALL_DIR}) ENDFOREACH(python) # --- INSTALL VISUALIZATION SCRIPTS SET(PYTHON_VISUALIZE_FILES __init__.py base_visualizer.py gepetto_visualizer.py meshcat_visualizer.py ) FOREACH(python ${PYTHON_VISUALIZE_FILES}) PYTHON_BUILD(${PROJECT_NAME}/visualize ${python}) INSTALL(FILES "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/pinocchio/visualize/${python}" DESTINATION ${${PYWRAP}_INSTALL_DIR}/visualize) ENDFOREACH(python) # --- PACKAGING --- # # Format string ... ...
 ... ... @@ -26,7 +26,19 @@ namespace pinocchio bp::args("model", "data", "geometry model", "geometry data"), "Update the placement of the collision objects according to the current joint placement stored in data." ); bp::def("setGeometryMeshScales", (void (*)(GeometryModel &, const Vector3d &))&setGeometryMeshScales, bp::args("geometry model", "scale"), "Set a mesh scaling vector to each GeometryObject contained in the the GeometryModel." ); bp::def("setGeometryMeshScales", (void (*)(GeometryModel &, const double))&setGeometryMeshScales, bp::args("geometry model", "scale"), "Set an isotropic mesh scaling to each GeometryObject contained in the the GeometryModel." ); #ifdef PINOCCHIO_WITH_HPP_FCL bp::def("computeCollision",computeCollision, bp::args("geometry model", "geometry data", "collision pair index"), ... ...
 ... ... @@ -9,19 +9,65 @@ namespace pinocchio { namespace python { Eigen::MatrixXd bodyRegressor_proxy(const Motion & v, const Motion & a) { return bodyRegressor(v,a); } Eigen::MatrixXd jointBodyRegressor_proxy(const Model & model, Data & data, const JointIndex jointId) { return jointBodyRegressor(model,data,jointId); } Eigen::MatrixXd frameBodyRegressor_proxy(const Model & model, Data & data, const FrameIndex frameId) { return frameBodyRegressor(model,data,frameId); } void exposeRegressor() { using namespace Eigen; bp::def("computeStaticRegressor", ®ressor::computeStaticRegressor, &computeStaticRegressor, bp::args("Model","Data", "Configuration q (size Model::nq)"), "Compute the static regressor that links the inertia parameters of the system to its center of mass position\n" ",put the result in Data and return it.", "Compute the static regressor that links the inertia parameters of the system to its center of mass position,\n" "put the result in Data and return it.", bp::return_value_policy()); bp::def("bodyRegressor", &bodyRegressor_proxy, bp::args("velocity","acceleration"), "Computes the regressor for the dynamic parameters of a single rigid body.\n" "The result is such that " "Ia + v x Iv = bodyRegressor(v,a) * I.toDynamicParameters()"); bp::def("jointBodyRegressor", &jointBodyRegressor_proxy, bp::args("Model","Data", "jointId (int)"), "Compute the regressor for the dynamic parameters of a rigid body attached to a given joint.\n" "This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects."); bp::def("frameBodyRegressor", &frameBodyRegressor_proxy, bp::args("Model","Data", "frameId (int)"), "Computes the regressor for the dynamic parameters of a rigid body attached to a given frame.\n" "This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects."); bp::def("computeJointTorqueRegressor", &computeJointTorqueRegressor, bp::args("Model","Data", "Configuration q (size Model::nq)", "Velocity v (size Model::nv)" "Acceleration a (size Model::nv)"), "Compute the joint torque regressor that links the joint torque " "to the dynamic parameters of each link according to the current the robot motion,\n" "put the result in Data and return it.", bp::return_value_policy()); } } // namespace python ... ...
 // // Copyright (c) 2015-2018 CNRS // Copyright (c) 2015-2019 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // ... ... @@ -12,6 +12,7 @@ #include "pinocchio/bindings/python/utils/version.hpp" #include "pinocchio/bindings/python/utils/dependencies.hpp" #include "pinocchio/bindings/python/utils/conversions.hpp" #include "pinocchio/bindings/python/utils/registration.hpp" namespace bp = boost::python; using namespace pinocchio::python; ... ... @@ -21,8 +22,11 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap) bp::scope().attr("__version__") = pinocchio::printVersion(); eigenpy::enableEigenPy(); eigenpy::exposeAngleAxis(); eigenpy::exposeQuaternion(); if(not register_symbolic_link_to_registered_type()) eigenpy::exposeQuaternion(); if(not register_symbolic_link_to_registered_type()) eigenpy::exposeAngleAxis(); typedef Eigen::Matrix Matrix6d; typedef Eigen::Matrix Vector6d; ... ... @@ -44,6 +48,7 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap) bp::enum_< ::pinocchio::ReferenceFrame >("ReferenceFrame") .value("WORLD",::pinocchio::WORLD) .value("LOCAL",::pinocchio::LOCAL) .value("LOCAL_WORLD_ALIGNED",::pinocchio::LOCAL_WORLD_ALIGNED) ; exposeModel(); ... ...
 ... ... @@ -108,6 +108,7 @@ namespace pinocchio bp::no_init) .def(GeometryDataPythonVisitor()) .def(PrintableVisitor()) .def(CopyableVisitor()) ; } ... ...
 ... ... @@ -9,6 +9,7 @@ #include "pinocchio/bindings/python/utils/eigen_container.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" #include "pinocchio/multibody/geometry.hpp" EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::GeometryModel) ... ... @@ -43,6 +44,7 @@ namespace pinocchio "Add a GeometryObject to a GeometryModel and set its parent joint by reading its value in model") .def("getGeometryId",&GeometryModel::getGeometryId) .def("existGeometryName",&GeometryModel::existGeometryName) .def("createData",&GeometryModelPythonVisitor::createData) #ifdef PINOCCHIO_WITH_HPP_FCL .add_property("collisionPairs", &GeometryModel::collisionPairs, ... ... @@ -71,7 +73,7 @@ namespace pinocchio ; } static GeometryData createData(const GeometryModel & geomModel) { return GeometryData(geomModel); } /* --- Expose --------------------------------------------------------- */ static void expose() ... ... @@ -81,6 +83,7 @@ namespace pinocchio bp::no_init) .def(GeometryModelPythonVisitor()) .def(PrintableVisitor()) .def(CopyableVisitor()) ; } ... ...
 ... ... @@ -172,7 +172,7 @@ namespace pinocchio .def("addJointFrame", &Model::addJointFrame, bp::args("jointIndex", "frameIndex"), "add the joint at index jointIndex as a frame to the frame tree") .def("appendBodyToJoint",&Model::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.") .def("addBodyFrame", &Model::addBodyFrame, bp::args("body_name", "parentJoint", "body_plaement", "previous_frame(parent frame)"), "add a body to the frame tree") .def("addBodyFrame", &Model::addBodyFrame, bp::args("body_name", "parentJoint", "body_placement", "previous_frame(parent frame)"), "add a body to the frame tree") .def("getBodyId",&Model::getBodyId, bp::args("name"), "Return the index of a frame of type BODY given by its name") .def("existBodyName", &Model::existBodyName, bp::args("name"), "Check if a frame of type BODY exists, given its name") .def("getJointId",&Model::getJointId, bp::args("name"), "Return the index of a joint given by its name") ... ...
 # Dealing with Lie group geometry Pinocchio is relying heavily on Lie groups and Lie algebra to handle motions and more specifically rotations. For this reason it supports the following special groups \$$SO(2), SO(3), SE(2), SE(3) \$$ and implements their associated algebra \f$( \mathfrak{se}(2) , \mathfrak{se}(3) \f$). Pinocchio relies heavily on Lie groups and Lie algebras to handle motions and more specifically rotations. For this reason it supports the following special groups \$$SO(2), SO(3), SE(2), SE(3) \$$ and implements their associated algebras \f$\mathfrak{se}(2) , \mathfrak{se}(3) \f$. It has various applications like representing the motion of a robot free flyer joint (typically the base of a mobile robot), or the motion of the robot links. The later is particularly useful for collision detection. It is also interesting to have general vector space over which a Lie algebra is defined. ## Using \$$SE(2) \$$ with pinocchio in C++ As a motivating example let us consider a mobile robot evolving in a plane \f$(\mathbb{R}^2 \times \mathbb{S}^1 \f$). ![SE2MotivatingExample.svg](SE2MotivatingExample.svg) ![SE2MotivatingExample](SE2MotivatingExample.svg) The robot starts at position \f$pose_s=(x_s,y_s,\theta_s) \f$ and after a rigid motion The robot starts at position \f$pose_s = (x_s,y_s,\theta_s) \f$ and after a rigid motion \f$\delta_u=(\delta x,\delta y,\delta \theta) \f$ it is finishing at position \f$pose_g = (x_g,y_g,\theta_g)\f$. it finishes at \f$pose_g = (x_{g},y_{g},\theta_{g})\f$. It is possible to instantiate the corresponding \$$SE(2)\$$ objects using: \code ... ... @@ -30,7 +31,7 @@ It is possible to instantiate the corresponding \$$SE(2)\$$ objects using: You can change Scalar by another type such as float. In this example, \f$pose_s=(1,1,\pi/4)\f$ and \f$pose_g=(3,1,-\pi/2) \f$ and we want to compute In this example, \f$pose_s=(1,1,\pi/4)\f$ and \f$pose_g=(3,1,-\pi/2) \f$ and we want to compute \f$\delta_u \f$ \code pose_s(0) = 1.0; pose_s(1) = 1.0; ... ... @@ -42,20 +43,23 @@ In this example, \f$pose_s=(1,1,\pi/4)\f$ and \f$pose_g=(3,1,-\pi/2) \f$ and we std::cout << delta_u << std::endl; \endcode aSE2 is used to compute the difference between two configuration vectors representing the two poses. Note that the rotation is represented by two numbers \f$(sin(\theta),cos(\theta))\f$ which is also a \f$SO(2) \f$ object. The difference lies in the tangent space of \f$SE(2)\f$ and is representend by a vector of 3 reals. aSE2 is used to compute the difference between two configuration vectors representing the two poses. Note that the rotation is represented by two numbers \f$sin(\theta),cos(\theta)\f$ which is also a \f$SO(2) \f$ object. The difference lies in the tangent space of \f$SE(2)\f$ and is representend by a vector of 3 reals. Therefore the output is: \code 3.33216 -1.38023 -2.35619 \endcode Note that the linear part is not following a straight path, it also takes into account that the system is rotating. We can verify that this is the appropriate motion by integrating: \code SpecialEuclideanOperationTpl<2,Scalar,Options>::ConfigVector_t pose_check; aSE2.integrate(pose_s,delta_u,pose_check); std::cout << pose_check << std::endl; \endcode ... ... @@ -69,5 +73,147 @@ The result is indeed: -1 \endcode ## Using \f$SE(3) \f$ with pinocchio in C++ Our mobile robot is not in a plane but in a 3-dimensional space. So let's consider a object in our physical space. This is actually almost the same case, we want the object from one position to an other position. The difficulty lies in the fact that we now have three dimensions so the object has six degrees of freedom, three corresponding to its translation and three to its rotation. ![SE3MotivatingExample](SE3Example1.jpg) It is also possible to instantiate the corresponding object which is now a \f$SE(3) \f$ object using the same algorithm and changing the dimension parameter: \code typedef double Scalar; enum {Options = 0}; SpecialEuclideanOperationTpl<3,Scalar,Options> aSE3 ; SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pose_s,pose_g; SpecialEuclideanOperationTpl<3,Scalar,Options>::TangentVector_t delta_u ; \endcode In this example, \f$pose_s=(1,1,1,\pi/2,\pi/4,\pi/8)\f$ and \f$pose_g=(4,3,3,\pi/4,\pi/3, -\pi) \f$. For the starting position, there is first a rotation around the y-axis then the x-axis and finally the z-axis. For the final position, the rotations are in this order, x-axis, y-axis, z-axis. We want to compute \f$\delta_u \f$. - For the first pose, we have the three rotations matrices for each rotation : \f$R_{x_s} = \begin{bmatrix} 1 &0 &0 \\ 0 &cos(\pi/8) &-sin(\pi/8) \\0 &sin(\pi/8) &cos(\pi/8) \end{bmatrix} \ \ R_{y_s} = \begin{bmatrix} cos(\pi/4) &0 &sin(\pi/4) \\ 0 &1 &0 \\-sin(\pi/4) &0 &cos(\pi/4) \end{bmatrix} \ \ R_{z_s} = \begin{bmatrix} cos(\pi/2) &-sin(\pi/2) &0 \\sin(\pi/2) &cos(\pi/2) &0 \\ 0 &0 &1\end{bmatrix} \f$ Therefore, the complete rotation is: \f$R_{pose_s} = R_{s_z} * R_{s_x} * R_{s_y} = \begin{bmatrix} 0 &-1 &0 \\ cos(\pi/4)*cos(\pi/8) + sin(\pi/4) * sin(\pi/8) &0 &sin(\pi/4) * cos(\pi/8) - cos(\pi/4) * sin(\pi/8) \\ sin(\pi/8) * cos(\pi/4) - cos(\pi/8) * sin(\pi/4) &0 &sin(\pi/4) * sin(\pi/8) + cos(\pi/4) * cos(\pi/8) \end{bmatrix} \f$ - For the second one, we have: \f$R_{x_g} = \begin{bmatrix} 1 &0 &0 \\ 0 &cos(\pi/4) &-sin(\pi/4) \\0 &sin(\pi/4) &cos(\pi/4) \end{bmatrix} \ \ R_{y_g} = \begin{bmatrix} cos(\pi/3) &0 &sin(\pi/3) \\ 0 &1 &0 \\ -sin(\pi/3) &0 &cos(\pi/3) \end{bmatrix} \ \ R_{z_g} = \begin{bmatrix} cos(-\pi) &-sin(-\pi) &0 \\ sin(-\pi) &cos(-\pi) &0 \\ 0 &0 &1\end{bmatrix} \f$ The complete rotation is: \f$R_{pose_g} = \begin{bmatrix} -cos(\pi/3) &-sin(\pi/3) * sin(\pi/4) &-cos(\pi/4) * sin(\pi/3) \\ 0 &-cos(\pi/4) &sin(\pi/4) \\ -sin(\pi/3) &sin(\pi/4) * cos(\pi/3) &cos(\pi/3) * cos(\pi/4) \end{bmatrix} \f$ To compute \f$\delta_u \f$ using Pinocchio we need to transform \f$R_{pose_s} \f$ and \f$R_{pose_g} \f$ matrices into quaternions using: \code float s = 0.5f / sqrtf(trace+ 1.0f); q.x = ( R[2][1] - R[1][2] ) * s; q.y = ( R[0][2] - R[2][0] ) * s; q.z = ( R[1][0] - R[0][1] ) * s; q.w = 0.25f / s; \endcode The quaternions components are: - For the first rotation \code 0.69352 -0.13795 0.13795 0.69352 \endcode - For the second one \code 0.191342 -0.46194 0.331414 0.800103 \endcode For each pose we have now a mathematical object with seven components and both are normalized. As for the \f$SE(2) \f$ example we compute \f$\delta_u \f$ using: \code pose_s(0) = 1.0; pose_s(1) = 1.0; pose_s(2) = 1 ; pose_s(3) = -0.13795 ; pose_s(4) = 0.13795; pose_s(5) = 0.69352; pose_s(6) = 0.69352; pose_g(0) = 4; pose_g(1) = 3 ; pose_g(2) = 3 ; pose_g(3) = -0.46194; pose_g(4) = 0.331414; pose_g(5) = 0.800103; pose_g(6) = 0.191342; aSE3.difference(pose_s,pose_g,delta_u); std::cout << delta_u << std::endl; \endcode The difference lies in the tangent space of \f$SE(3)\f$ and is represented by a vector of 6 reals which is: \code -1.50984 -3.58755 2.09496 -0.374715 0.887794 0.86792 \endcode The three first values are linear and the three last are velocities. To verify it is the good solution, we integrate: \code SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pose_check; aSE3.integrate(pose_s,delta_u,pose_check); std::cout << pose_check << std::endl; \endcode Indeed, we find : \code 4 3 3 -0.46194 0.331414 0.800103 0.191234 \endcode ## Using interpolation to plot a trajectory with pinocchio in C++ Assuming that we want to make the robot pass through known positions, we can use interpolations to plot a trajectory. The problem is an interpolation such as Lagrange's one only takes into account translations whlie the robot interact with its environment by performing translations and rotations. A possibility is to use the \f$\delta_{theta} \f$ method by using quaternions. The method is simple, we just vary the angle, the scalar component of the quaternion, with very small variations. Let's consider the previous example, we can interpolate trajectory using: \code SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pole; aSE3.interpolate(pose_s,pose_g,0.5f, pole); std::cout << pole << std::endl; \endcode The output corresponds to the middle of the trajectory and is: \code 2.7486 1.4025 2.22461 -0.316431 0.247581 0.787859 0.466748 \endcode
 ... ... @@ -2,6 +2,69 @@ ## Geometry A rigid body system is an assembly of different parts which are joints, rigid bodies and forces. A joint connects two different bodies and gather all kinematic relations between those two bodies, allowing the creation of a relative displacement between the two bodies. This displacement is described by breaking it down into three parts, rotations, translations or the compositions of a rotation and a translation. Rotation matrices form the so-called **Special Orthogonal** group \f$SO-n) \f$. There are two groups within the latter which interest us as for now: \f$SO(2) \f$ and \f$SO(3) \f$. \f$SO(3) \f$ is the group of all rotations in the 3-dimensionnal space. Its elements are matrices of size 3 by 3. \f$SO(3) \f$ is useful for planar problems. It is the group of rotations in the 2-dimensionnal space. Its elements are matrices of size 2 by 2. The set that brings together all the homogeneous transformations matrices is the **Special Euclidean** group \f$SE(n) \f$. As with rotation matrices, there are two different groups, \f$SE(3) \f$ for 3-dimensional transformations and \f$SE(2) \f$ for 2-dimensional transformation, i.e. transformation in a plane. ### Using quaternions for a \f$SO(3) \f$ object To use quaternions for a \f$SO(3) \f$ object we have several methods, we can do as in the \f$SE(3) \f$ example in the **e-lie** chapter by removing the translation vector. Or we can just consider one rotation instead of two. For example, in a landmark link to the robot itself, we consider the starting position as the origin of this landmark. So let's consider a cube in the 3-dimensional space. ![A rotation around its diagonal](cube_rotation.gif) ![Position and Landmark](cube-rotation_picture.jpg) We want to determine the image of a vector \f$\overrightarrow{v} \f$ by a \f$120° \f$ rotation (\f$\frac{2\pi}{3}) \f$ around the big diagonal of the cube, let's call it \f$\overrightarrow{r} \f$. We have to use a passage through quaternion. We have\f$\overrightarrow{v} = \overrightarrow{i} + \overrightarrow{k} = \begin{pmatrix} 1 \\ 0 \\ 1 \end{pmatrix} \f$ and \f$\overrightarrow{r} = \overrightarrow{i} + \overrightarrow{j} + \overrightarrow{k} = \begin{pmatrix} 1 \\ 1 \\ 1 \end{pmatrix} \f$ We compute the corresponding quaternion: \f$q = cos(\alpha/2) + sin(\alpha/2) * \frac{\overrightarrow{r}}{||\overrightarrow{r}||} \f$ Therefore we have: \f$q = \frac{1}{2} + \frac{1}{2} * (\overrightarrow{i} + \overrightarrow{j} + \overrightarrow{k}) \f$ And so we can compute the image of vector \f$\overrightarrow{v} \f$ using: \f$\overrightarrow{v'} = q * \overrightarrow{v} *q^{-1} \f$ we have: \f$\overrightarrow{v'} = \overrightarrow{i} + \overrightarrow{j} = \begin{pmatrix} 1 \\ 1 \\ 0 \end{pmatrix} \f$ ### Benefits of using quaternions Determining the matrix corresponding to a rotation is not immediate, so that very often the physical problem is defined by the couple \f$(\alpha,\overrightarrow{r} ) \f$. Another problem related to the composition of rotations is known as "blocking of Cardan" : we can see it in specific cases, for example when two successive joints have close or even aligned axes of rotation. In this case, a very large variation of the first angle does not change the position of the end of the device. A robot could then generate very strong violent movements without realizing it, due to the approximation of the calculations. To remedy these two points, we use quaternions. ### Cartesian product Of course the cartesian product is essential for analysis and description of the movement in our Euclidean space. But here, it's specific to the lie algebra, this is different from the cartesian product which define our space. The cartesian product can also be used to create a specific space by associating spaces related to the lie algebra as \f$SE(n) \f$ and \f$SO(n) \f$ groups. For example let's consider a wheeled robot like Tiago. It can only move on the ground. It is possible to assimilate the ground as a plane. The robot can rotate around the z-axis so we have to deal with a \f$SE(2) \f$ object. Then we attach to this \f$SE(2) \f$ object an articulated arm with four revolute joints spread out his arm, each has one degree of freedom of rotation so they are \f$SO(2) \f$ objects. To deal with this set we use the cartesian product related to the lie algebra and we get a new space in which we are able to represent all the possible trajectories of the robot and its arm. ### Vector space If you want to create a tangent space to simplify calculations of a trajectory it is necessary to use vector spaces. Indeed, a tangent space is a vector space that is the whole of all velocity vectors. Let's consider an object having a trajectory, all points of it have a velocity which is tangent to the trajectory and the space associate to one velocity and passing by one point of the trajectory is the \b tangent \b space. Furthermore, by using vector spaces we have the possibility to use its properties as those of the Euclidean cross operator and linear combinations. However it is important to know that "vector space" is here related to **Lie algebra** and this is different for a vector space we used to deal with. ## Kinematics ## Dynamics
 ... ... @@ -2,10 +2,104 @@ ## Geometry Joints are not simple objects, it can be difficult to deal with. To facilitate their description Pinocchio use the lie algebra which is describe in the *Dealing with Lie bgroup geometry * chapter. Let's take the base joints and express them using lie algebra : - the **revolute** joint is a \f$SO(2)\f$ object. \f$Mat_{move} = \begin{bmatrix} 0\\0\\1\\0\\0\\0 \end{bmatrix} \ \ Mat_{cons} = \begin{bmatrix} 1 &0 &0 &0 &0 \\ 0 &1 &0 &0 &0 \\0 &0 &0 &0 &0 \\0 &0 &1 &0 &0 \\ 0 &0 &0 &1 &0 \\0 &0 &0 &0 &1 \end{bmatrix} \f$ - the **cylindrical** joint : \f$Mat_{move} = \begin{bmatrix} 0 &0 \\ 0 &0 \\ 1 &0 \\ 0 &0 \\ 0 &0 \\ 0 &1 \end{bmatrix} \ \ Mat_{const} = \begin{bmatrix} 1 &0 &0 &0 \\ 0 &1 &0 &0 \\ 0 &0 &1 &0 \\ 0 &0 &0 &1 \\ 0 &0 &0 &0 \end{bmatrix} \f$ - The **spherical** joint is a \f$SO(3) \f$ object. \f$Mat_{move} = \begin{bmatrix} 1 &0 &0\\0 &1 &0\\0 &0 &1\\0 &0 &0\\0 &0 &0\\0 &0 &0 \end{bmatrix} \ \ Mat_{cons} = \begin{bmatrix} 0 &0 &0 \\ 0 &0 &0 \\0 &0 &0 \\1 &0 &0 \\ 0 &1 &0 \\0 &0 &1 \end{bmatrix} \f$ - The **planar** joint is a \f$SE(2) \f$ object. \f$Mat_{move} = \begin{bmatrix} 0 &0 &0\\0 &0 &0\\0 &0 &0\\1 &0 &0\\0 &1 &0\\0 &0 &1 \end{bmatrix} \ \ Mat_{cons} = \begin{bmatrix} 1 &0 &0 \\ 0 &1 &0 \\0 &0 &1 \\0 &0 &0 \\ 0 &0 &0 \\0 &0 &0 \end{bmatrix} \f$ - The **free floating** joint is a \f$SE (3) \f$ object. The movement and constraints matrices are 6 by 6 matrices. \f\$ Mat_{move} =