interpolation-SE3.cpp

Go to the documentation of this file.

#include <iostream>
#include "pinocchio/multibody/liegroup/liegroup.hpp"

using namespace pinocchio;

int main ()
{ 
  typedef double Scalar;

  typedef SpecialEuclideanOperationTpl<3,Scalar> SE3Operation;
  SE3Operation aSE3;
  SE3Operation::ConfigVector_t pose_s,pose_g;
  SE3Operation::TangentVector_t delta_u ;  

  // Starting configuration
  pose_s(0) = 1.0; pose_s(1) = 1.0;
  pose_s(2) = 1 ; pose_s(3) = -0.13795 ; 
  pose_s(4) = 0.13795; pose_s(5) = 0.69352; pose_s(6) = 0.69352;
  aSE3.normalize(pose_s);

  // Goal configuration
  pose_g(0) = 4; pose_g(1) = 3;
  pose_g(2) = 3 ; pose_g(3) = -0.46194;
  pose_g(4) = 0.331414; pose_g(5) = 0.800103; pose_g(6) = 0.191342; 
  aSE3.normalize(pose_g);

  SE3Operation::ConfigVector_t pole_u;
  aSE3.interpolate(pose_s,pose_g,0.5, pole_u);
  std::cout << "Interpolated configuration: " << pole_u.transpose() << std::endl;

  return 0;
}