GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: examples/interpolation-SE3.cpp Lines: 16 16 100.0 %
Date: 2024-01-23 21:41:47 Branches: 26 52 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
10
  typedef SpecialEuclideanOperationTpl<3,Scalar> SE3Operation;
11
1
  SE3Operation aSE3;
12

1
  SE3Operation::ConfigVector_t pose_s,pose_g;
13
1
  SE3Operation::TangentVector_t delta_u ;
14
15
  // Starting configuration
16

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

1
  pose_s(2) = 1 ; pose_s(3) = -0.13795 ;
18

1
  pose_s(4) = 0.13795; pose_s(5) = 0.69352; pose_s(6) = 0.69352;
19
1
  aSE3.normalize(pose_s);
20
21
  // Goal configuration
22

1
  pose_g(0) = 4; pose_g(1) = 3;
23

1
  pose_g(2) = 3 ; pose_g(3) = -0.46194;
24

1
  pose_g(4) = 0.331414; pose_g(5) = 0.800103; pose_g(6) = 0.191342;
25
1
  aSE3.normalize(pose_g);
26
27
1
  SE3Operation::ConfigVector_t pole_u;
28
1
  aSE3.interpolate(pose_s,pose_g,0.5, pole_u);
29


1
  std::cout << "Interpolated configuration: " << pole_u.transpose() << std::endl;
30
31
1
  return 0;
32
}
33