Commit 05f4d164 by Thomas Douin

### [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 #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 #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!