[Device] Remove method integrate.

Author: florent-lamiraux

include/sot/core/device.hh

@@ -145,24 +145,8 @@ class SOT_CORE_EXPORT Device : public Entity {
   /// \}
 
  protected:
-  /// Compute roll pitch yaw angles of freeflyer joint.
-  void integrateRollPitchYaw(dynamicgraph::Vector &state,
-                             const dynamicgraph::Vector &control, double dt);
   /// Store Position of free flyer joint
   MatrixHomogeneous ffPose_;
-  /// Compute the new position, from the current control.
-  ///
-  /// When sanity checks are enabled, this checks that the control is within
-  /// bounds. There are three cases, depending on what the control is:
-  /// - position: checks that the position is within bounds,
-  /// - velocity: checks that the velocity and the future position are
-  ///             within bounds,
-  /// - acceleration: checks that the acceleration, the future velocity and
-  ///                 position are within bounds.
-  ///                 \todo in order to check the acceleration, we need
-  ///                 pinocchio and the contact forces in order to estimate
-  ///                 the joint torques for the given acceleration.
-  virtual void integrate(const double &dt);
 
  protected:
   /// Get freeflyer pose

src/tools/device.cpp

@@ -69,31 +69,6 @@ inline double saturateBounds(double &val, const double &lower,
 /* --- CLASS ----------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-void Device::integrateRollPitchYaw(Vector &state, const Vector &control,
-                                   double dt) {
-  using Eigen::AngleAxisd;
-  using Eigen::QuaternionMapd;
-  using Eigen::Vector3d;
-
-  typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
-  Eigen::Matrix<double, 7, 1> qin, qout;
-  qin.head<3>() = state.head<3>();
-
-  QuaternionMapd quat(qin.tail<4>().data());
-  quat = AngleAxisd(state(5), Vector3d::UnitZ()) *
-         AngleAxisd(state(4), Vector3d::UnitY()) *
-         AngleAxisd(state(3), Vector3d::UnitX());
-
-  SE3().integrate(qin, control.head<6>() * dt, qout);
-
-  // Update freeflyer pose
-  ffPose_.translation() = qout.head<3>();
-  state.head<3>() = qout.head<3>();
-
-  ffPose_.linear() = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
-  state.segment<3>(3) = ffPose_.linear().eulerAngles(2, 1, 0).reverse();
-}
-
 const MatrixHomogeneous &Device::freeFlyerPose() const { return ffPose_; }
 
 Device::~Device() {
@@ -382,9 +357,6 @@ void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
 }
 
 
-void Device::integrate(const double &dt) {
-}
-
 /* --- DISPLAY ------------------------------------------------------------ */
 
 void Device::display(std::ostream &os) const {