Commit 05f4d164 authored by Thomas Douin's avatar Thomas Douin
Browse files

[doc] modified interpolation for SE3

parent ac5997c8
......@@ -116,10 +116,10 @@ To compute \f$ \delta_u \f$ using Pinocchio we need to transform \f$ R_{pose_s}
\code
float s = 0.5f / sqrtf(trace+ 1.0f);
q.w = 0.25f / s;
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 :
......@@ -139,7 +139,7 @@ The quaternions components are :
0.800103
\endcode
For each pose we have now a mathematical object with seven components. As for the \f$ SE(2) \f$ example we compute \f$ \delta_u \f$ using :
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;
......@@ -164,6 +164,7 @@ The difference lies in the tangent space of \f$ SE(3)\f$ and is representend by
0.86792
\endcode
The three first values are linear and the three last are velocities.
To verify it is the good solution, we integrate :
......@@ -199,19 +200,19 @@ A possibility is to use the \f$ \delta_{theta} \f$ method by using quaternions.
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,2.0f, pole);
aSE3.interpolate(pose_s,pose_g,0.5f, pole);
std::cout << pole << std::endl;
\endcode
The output is :
The output correspond to the middle of the trajectory and is :
\code
3.69572
6.96909
1.92584
-0.59841
0.390343
0.581891
-0.38851
2.7486
1.4025
2.22461
-0.316431
0.247581
0.787859
0.466748
\endcode
......
......@@ -5,6 +5,8 @@
SET(${PROJECT_NAME}_EXAMPLES
overview-simple
overview-lie
overview-SE3
interpolation-SE3
#overview-urdf
)
......
#include <iostream>
#include "pinocchio/multibody/liegroup/liegroup.hpp"
using namespace pinocchio;
int main ()
{
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 ;
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;
SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pole_u;
aSE3.interpolate(pose_s,pose_g,0.5f, pole_u);
std::cout << pole_u << std::endl;
return 0;
}
\ No newline at end of file
#include <iostream>
#include "pinocchio/multibody/liegroup/liegroup.hpp"
using namespace pinocchio;
int main ()
{
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 ;
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;
SpecialEuclideanOperationTpl<3,Scalar,Options>::ConfigVector_t pose_check;
aSE3.integrate(pose_s,delta_u,pose_check);
std::cout << pose_check << std::endl;
return 0;
}
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment