overview-SE3.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include "pinocchio/multibody/liegroup/liegroup.hpp"
3 
4 using namespace pinocchio;
5 int main ()
6 {
7  typedef double Scalar;
8  typedef SpecialEuclideanOperationTpl<3,Scalar> SE3Operation;
9 
10  SE3Operation aSE3;
11  SE3Operation::ConfigVector_t pose_s,pose_g;
12  SE3Operation::TangentVector_t delta_u ;
13 
14  pose_s(0) = 1.0; pose_s(1) = 1.0;
15  pose_s(2) = 1 ; pose_s(3) = -0.13795 ;
16  pose_s(4) = 0.13795; pose_s(5) = 0.69352; pose_s(6) = 0.69352;
17  pose_g(0) = 4; pose_g(1) = 3 ;
18  pose_g(2) = 3 ; pose_g(3) = -0.46194;
19  pose_g(4) = 0.331414; pose_g(5) = 0.800103; pose_g(6) = 0.191342;
20 
21  // First normalize the inputs
22  aSE3.normalize(pose_s);
23  std::cout << "pose_s: " << pose_s.transpose() << std::endl;
24  aSE3.normalize(pose_g);
25  std::cout << "pose_g: " << pose_g.transpose() << std::endl;
26 
27  aSE3.difference(pose_s,pose_g,delta_u);
28  std::cout << "delta_u: " << delta_u.transpose() << std::endl;
29 
30  SE3Operation::ConfigVector_t pose_check;
31 
32  aSE3.integrate(pose_s,delta_u,pose_check);
33  std::cout << "pose_check: " << pose_check.transpose() << std::endl;
34 
35  return 0;
36 }
37 
SE3::Scalar Scalar
Definition: conversions.cpp:13
Main pinocchio namespace.
Definition: timings.cpp:28
int main()
Definition: overview-SE3.cpp:5


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32