From 714369193c4bd9a6250dd9821a7e54597d5fc331 Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Sun, 22 Jul 2018 18:10:25 -0500 Subject: [PATCH] Added a custom routine to axial symmetric pose to sample points about the given free-axis. Previously it forwarded to Cartesian pt's impl that used Euler angles and the result was just wrong for any free-axis that wasn't Z. --- descartes_core/include/descartes_core/utils.h | 2 +- .../descartes_trajectory/axial_symmetric_pt.h | 9 +- .../src/axial_symmetric_pt.cpp | 112 ++++++++++++------ 3 files changed, 83 insertions(+), 40 deletions(-) diff --git a/descartes_core/include/descartes_core/utils.h b/descartes_core/include/descartes_core/utils.h index 12f5fc56..7699098f 100644 --- a/descartes_core/include/descartes_core/utils.h +++ b/descartes_core/include/descartes_core/utils.h @@ -21,7 +21,7 @@ #include #include -#include +#include /** \def DESCARTES_CLASS_FORWARD Macro that forward declares a class XXX, and also defines two shared ptrs with named XXXPtr and XXXConstPtr */ diff --git a/descartes_trajectory/include/descartes_trajectory/axial_symmetric_pt.h b/descartes_trajectory/include/descartes_trajectory/axial_symmetric_pt.h index 52e7fb2d..e9164d35 100644 --- a/descartes_trajectory/include/descartes_trajectory/axial_symmetric_pt.h +++ b/descartes_trajectory/include/descartes_trajectory/axial_symmetric_pt.h @@ -72,10 +72,17 @@ class AxialSymmetricPt : public descartes_trajectory::CartTrajectoryPt AxialSymmetricPt(const Eigen::Affine3d& pose, double orient_increment, FreeAxis axis, const descartes_core::TimingConstraint& timing = descartes_core::TimingConstraint()); - virtual descartes_core::TrajectoryPtPtr copy() const + descartes_core::TrajectoryPtPtr copy() const override { return descartes_core::TrajectoryPtPtr(new AxialSymmetricPt(*this)); } + + void getJointPoses(const descartes_core::RobotModel &model, std::vector> &joint_poses) const override; + + void getCartesianPoses(const descartes_core::RobotModel &model, EigenSTL::vector_Affine3d &poses) const override; + +private: + FreeAxis axis_; }; } // descartes trajectory diff --git a/descartes_trajectory/src/axial_symmetric_pt.cpp b/descartes_trajectory/src/axial_symmetric_pt.cpp index c34f0f84..5ffa1723 100644 --- a/descartes_trajectory/src/axial_symmetric_pt.cpp +++ b/descartes_trajectory/src/axial_symmetric_pt.cpp @@ -4,42 +4,9 @@ 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(x, y, z); - OrientationTolerance orient_tol = ToleranceBase::createSymmetric( - ((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(x, y, z); - OrientationTolerance orient_tol = ToleranceBase::createSymmetric( - ((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) { } @@ -47,20 +14,89 @@ AxialSymmetricPt::AxialSymmetricPt(const descartes_core::TimingConstraint& timin 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), + : CartTrajectoryPt(TolerancedFrame(toFrame(x, y, z, rx, ry, rz)), 0.0, // The position discretization orient_increment, // Orientation discretization (starting at -2Pi and marching to 2Pi) timing) -{ -} + , axis_{axis} +{} AxialSymmetricPt::AxialSymmetricPt(const Eigen::Affine3d& pose, double orient_increment, FreeAxis axis, const descartes_core::TimingConstraint& timing) - : CartTrajectoryPt(makeUnconstrainedRotation(pose, axis), + : CartTrajectoryPt(TolerancedFrame(pose), 0.0, // The position discretization orient_increment, // Orientation discretization (starting at -2Pi and marching to 2Pi) timing) + , axis_{axis} +{} + +static void computePoses(const Eigen::Affine3d& origin, const AxialSymmetricPt::FreeAxis& axis, + const double orientation_step, EigenSTL::vector_Affine3d& out) { + Eigen::Vector3d rot_axis; + switch (axis) + { + case AxialSymmetricPt::FreeAxis::X_AXIS: + rot_axis = Eigen::Vector3d::UnitX(); + break; + case AxialSymmetricPt::FreeAxis::Y_AXIS: + rot_axis = Eigen::Vector3d::UnitY(); + break; + case AxialSymmetricPt::FreeAxis::Z_AXIS: + rot_axis = Eigen::Vector3d::UnitZ(); + break; + } + + const static double lower_limit = -M_PI; + const static double upper_limit = M_PI; + + out.clear(); + + const auto n_elements = static_cast((upper_limit - lower_limit) / orientation_step) + 1u; + out.reserve(n_elements); + + double s = lower_limit; + for (std::size_t i = 0; i < n_elements; ++i) + { + out.push_back(origin * Eigen::AngleAxisd(s, rot_axis)); + s += orientation_step; + } +} + +void AxialSymmetricPt::getJointPoses(const descartes_core::RobotModel &model, + std::vector>& joint_poses) const +{ + joint_poses.clear(); + + EigenSTL::vector_Affine3d poses; + computePoses(this->wobj_pt_.frame, axis_, this->orient_increment_, poses); + + joint_poses.reserve(poses.size()); + for (const auto& pose : poses) + { + std::vector> sols; + if (model.getAllIK(pose, sols)) + { + joint_poses.insert(joint_poses.end(), std::make_move_iterator(sols.begin()), + std::make_move_iterator(sols.end())); + } + } +} + +void AxialSymmetricPt::getCartesianPoses(const descartes_core::RobotModel &model, + EigenSTL::vector_Affine3d &poses) const +{ + // Computes all of the candidate poses then filters them based on IK - not sure how I feel about this but I'll keep + // the behaviour already present in getCartesianPoses + poses.clear(); + EigenSTL::vector_Affine3d all_poses; + computePoses(wobj_pt_.frame, axis_, orient_increment_, all_poses); + + poses.reserve(all_poses.size()); + for (const auto& pose : all_poses) + { + if (model.isValid(pose)) poses.push_back(pose); + } } } // end of namespace descartes_trajectory