axial_symmetric_pt.cpp

Go to the documentation of this file.

#include "descartes_trajectory/axial_symmetric_pt.h"

using descartes_trajectory::TolerancedFrame;
using descartes_trajectory::AxialSymmetricPt;
using namespace descartes_core::utils;

static TolerancedFrame makeUnconstrainedRotation(double x, double y, double z, double rx, double ry, double rz,
                                                 AxialSymmetricPt::FreeAxis axis)
{
  using namespace descartes_trajectory;

  Eigen::Affine3d pose = toFrame(x, y, z, rx, ry, rz, EulerConventions::XYZ);
  PositionTolerance pos_tol = ToleranceBase::zeroTolerance<PositionTolerance>(x, y, z);
  OrientationTolerance orient_tol = ToleranceBase::createSymmetric<OrientationTolerance>(
      ((axis == AxialSymmetricPt::X_AXIS) ? 0.0 : rx), ((axis == AxialSymmetricPt::Y_AXIS) ? 0.0 : ry),
      ((axis == AxialSymmetricPt::Z_AXIS) ? 0.0 : rz), ((axis == AxialSymmetricPt::X_AXIS) ? 2 * M_PI : 0.0),
      ((axis == AxialSymmetricPt::Y_AXIS) ? 2 * M_PI : 0.0), ((axis == AxialSymmetricPt::Z_AXIS) ? 2 * M_PI : 0.0));
  return TolerancedFrame(pose, pos_tol, orient_tol);
}

static TolerancedFrame makeUnconstrainedRotation(const Eigen::Affine3d& pose, AxialSymmetricPt::FreeAxis axis)
{
  using namespace descartes_trajectory;

  Eigen::Vector3d rpy = pose.rotation().eulerAngles(0, 1, 2);
  double rx = rpy(0);
  double ry = rpy(1);
  double rz = rpy(2);
  double x = pose.translation()(0);
  double y = pose.translation()(1);
  double z = pose.translation()(2);

  PositionTolerance pos_tol = ToleranceBase::zeroTolerance<PositionTolerance>(x, y, z);
  OrientationTolerance orient_tol = ToleranceBase::createSymmetric<OrientationTolerance>(
      ((axis == AxialSymmetricPt::X_AXIS) ? 0.0 : rx), ((axis == AxialSymmetricPt::Y_AXIS) ? 0.0 : ry),
      ((axis == AxialSymmetricPt::Z_AXIS) ? 0.0 : rz), ((axis == AxialSymmetricPt::X_AXIS) ? 2 * M_PI : 0.0),
      ((axis == AxialSymmetricPt::Y_AXIS) ? 2 * M_PI : 0.0), ((axis == AxialSymmetricPt::Z_AXIS) ? 2 * M_PI : 0.0));
  return TolerancedFrame(pose, pos_tol, orient_tol);
}

namespace descartes_trajectory
{
AxialSymmetricPt::AxialSymmetricPt(const descartes_core::TimingConstraint& timing) : CartTrajectoryPt(timing)
{
}

AxialSymmetricPt::AxialSymmetricPt(double x, double y, double z, double rx, double ry, double rz,
                                   double orient_increment, FreeAxis axis,
                                   const descartes_core::TimingConstraint& timing)
  : CartTrajectoryPt(makeUnconstrainedRotation(x, y, z, rx, ry, rz, axis),
                     0.0,               // The position discretization
                     orient_increment,  // Orientation discretization (starting at -2Pi and marching to 2Pi)
                     timing)
{
}

AxialSymmetricPt::AxialSymmetricPt(const Eigen::Affine3d& pose, double orient_increment, FreeAxis axis,
                                   const descartes_core::TimingConstraint& timing)
  : CartTrajectoryPt(makeUnconstrainedRotation(pose, axis),
                     0.0,               // The position discretization
                     orient_increment,  // Orientation discretization (starting at -2Pi and marching to 2Pi)
                     timing)
{
}

}  // end of namespace descartes_trajectory