overview-lie.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include "pinocchio/multibody/liegroup/liegroup.hpp"
3 
4 using namespace pinocchio;
5 
6 int main()
7 {
8  typedef double Scalar;
9  enum {Options = 0};
10 
12  SE2Operation aSE2;
13  SE2Operation::ConfigVector_t pose_s,pose_g;
14  SE2Operation::TangentVector_t delta_u;
15 
16  // Starting configuration
17  pose_s(0) = 1.0; pose_s(1) = 1.0;
18  pose_s(2) = cos(M_PI/4.0); pose_s(3) = sin(M_PI/4.0);
19 
20  // Goal configuration
21  pose_g(0) = 3.0; pose_g(1) = -1.0;
22  pose_g(2) = cos(-M_PI/2.0); pose_g(3) = sin(-M_PI/2.0);
23 
24  // Computes the differences (expressed in the tangent space of the configuration space) between
25  // the starting and the goal configuration
26  aSE2.difference(pose_s,pose_g,delta_u);
27  std::cout << "difference: " << delta_u.transpose() << std::endl;
28 
29  // Check that the composition of the starting configuration and the difference vector gives the goal configuration
30  SE2Operation::ConfigVector_t pose_check;
31  aSE2.integrate(pose_s,delta_u,pose_check);
32  std::cout << "goal configuration (from composition): " << pose_check.transpose() << std::endl;
33  std::cout << "goal configuration: " << pose_g.transpose() << std::endl;
34 }
35 
#define M_PI
SE3::Scalar Scalar
Definition: conversions.cpp:13
Main pinocchio namespace.
Definition: timings.cpp:28
int main()
Definition: overview-lie.cpp:6


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