GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: examples/overview-lie.cpp Lines: 16 16 100.0 %
Date: 2024-04-26 13:14:21 Branches: 28 56 50.0 %

Line Branch Exec Source
1
#include <iostream>
2
#include "pinocchio/multibody/liegroup/liegroup.hpp"
3
4
using namespace pinocchio;
5
6
1
int main()
7
{
8
  typedef double Scalar;
9
  enum {Options = 0};
10
11
  typedef SpecialEuclideanOperationTpl<2,Scalar,Options> SE2Operation;
12
1
  SE2Operation aSE2;
13

1
  SE2Operation::ConfigVector_t pose_s,pose_g;
14
1
  SE2Operation::TangentVector_t delta_u;
15
1
  delta_u.setZero();
16
17
  // Starting configuration
18

1
  pose_s(0) = 1.0; pose_s(1) = 1.0;
19

1
  pose_s(2) = cos(M_PI/4.0); pose_s(3) = sin(M_PI/4.0);
20
21
  // Goal configuration
22

1
  pose_g(0) = 3.0; pose_g(1) = -1.0;
23

1
  pose_g(2) = cos(-M_PI/2.0); pose_g(3) = sin(-M_PI/2.0);
24
25
  // Computes the differences (expressed in the tangent space of the configuration space) between
26
  // the starting and the goal configuration
27
1
  aSE2.difference(pose_s,pose_g,delta_u);
28


1
  std::cout << "difference: " << delta_u.transpose() << std::endl;
29
30
  // Check that the composition of the starting configuration and the difference vector gives the goal configuration
31
1
  SE2Operation::ConfigVector_t pose_check;
32
1
  aSE2.integrate(pose_s,delta_u,pose_check);
33


1
  std::cout << "goal configuration (from composition): " << pose_check.transpose() << std::endl;
34


1
  std::cout << "goal configuration: " << pose_g.transpose() << std::endl;
35
1
}
36