interpolation-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 
6 int main ()
7 {
8  typedef double Scalar;
9 
10  typedef SpecialEuclideanOperationTpl<3,Scalar> SE3Operation;
11  SE3Operation aSE3;
12  SE3Operation::ConfigVector_t pose_s,pose_g;
13  SE3Operation::TangentVector_t delta_u ;
14 
15  // Starting configuration
16  pose_s(0) = 1.0; pose_s(1) = 1.0;
17  pose_s(2) = 1 ; pose_s(3) = -0.13795 ;
18  pose_s(4) = 0.13795; pose_s(5) = 0.69352; pose_s(6) = 0.69352;
19  aSE3.normalize(pose_s);
20 
21  // Goal configuration
22  pose_g(0) = 4; pose_g(1) = 3;
23  pose_g(2) = 3 ; pose_g(3) = -0.46194;
24  pose_g(4) = 0.331414; pose_g(5) = 0.800103; pose_g(6) = 0.191342;
25  aSE3.normalize(pose_g);
26 
27  SE3Operation::ConfigVector_t pole_u;
28  aSE3.interpolate(pose_s,pose_g,0.5, pole_u);
29  std::cout << "Interpolated configuration: " << pole_u.transpose() << std::endl;
30 
31  return 0;
32 }
33 
int main()
SE3::Scalar Scalar
Definition: conversions.cpp:13
Main pinocchio namespace.
Definition: timings.cpp:28


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