fix comparisons, publish on release

MiguelASTavares · Oct 21, 2020 · 72a9dce · 72a9dce
1 parent 2b8bc65
commit 72a9dce
Show file tree

Hide file tree

Showing 10 changed files with 67 additions and 93 deletions.
diff --git a/.github/workflows/publish.yml b/.github/workflows/publish.yml
@@ -1,9 +1,8 @@
 name: Publish
 
 on:
-  push:
-    branches:
-      - master
+  release:
+    types: [created]
 
 jobs:
   publish:
@@ -98,7 +97,6 @@ jobs:
         ls dist
 
     - name: publish python package
-      if: github.event_name == 'push' && startsWith(github.event.ref, 'refs/tags')
       uses: pypa/gh-action-pypi-publish@master
       with:
         user: keNB2

diff --git a/examples/grasping.cpp b/examples/grasping.cpp
@@ -18,7 +18,7 @@ int main(int argc, char *argv[]) {
     robot.move(joint_motion, joint_data);
 
     auto data = MotionData().withDynamicRel(0.5).withReaction(Reaction(
-        Measure::ForceZ, Comparison::Smaller, -7.0,
+        Measure::ForceZ, Comparison::Less, -7.0,
         std::make_shared<LinearRelativeMotion>(Affine(0.0, 0.0, 0.001))
     ));
 

diff --git a/examples/grasping.py b/examples/grasping.py
@@ -36,7 +36,7 @@ def get_base(x, y, z, a=0.0, b=0.0, c=0.0):
     def grasp(self, grasp: Grasp):
         data_down = MotionData(0.5)
         # .with_reaction(Reaction(
-        #     Measure.ForceZ, Comparison.Smaller, -7.0,
+        #     Measure.ForceZ, Comparison.Less, -7.0,
         #     LinearRelativeMotion(Affine(0.0, 0.0, 0.01))
         # ))
         data_up = MotionData(0.8)

diff --git a/include/frankx/quintic_generator.hpp b/include/frankx/quintic_generator.hpp
@@ -7,7 +7,7 @@ namespace frankx {
 
 template<size_t DOFs>
 class TrajectoryGenerator {
-    enum State {
+    enum Result {
         Working,
         Finished,
         Error
@@ -16,6 +16,9 @@ class TrajectoryGenerator {
     typedef Eigen::Matrix<double, DOFs, 1> Vector;
 
     // Current trajectory
+    Vector x0, v0, a0;
+    Vector xf, vf, af;
+
     Vector a, b, c, d, e, f;
     double t, tf;
 
@@ -25,6 +28,13 @@ class TrajectoryGenerator {
     TrajectoryGenerator(double delta_time): delta_time(delta_time) { }
 
     bool calculate(Vector x0, Vector v0, Vector a0, Vector xf, Vector vf, Vector af, Vector v_max) {
+        this->x0 = x0;
+        this->v0 = v0;
+        this->a0 = a0;
+        this->xf = xf;
+        this->vf = vf;
+        this->af = af;
+
         auto tfs = (15 * std::abs(x0 - xf)) / (8 * v_max);  // Approximation for v0 == 0, vf == 0, a0 == 0, af == 0
         tf = tfs.maxCoeff();
         t = 0.0;
@@ -39,15 +49,20 @@ class TrajectoryGenerator {
         return true;
     }
 
-    State update(Vector& output) {
+    Result update(Vector& pos_output, Vector& vel_output, Vector& acc_output) {
         t += delta_time;
 
         if (t >= tf) {
-            return State::Finished;
+            pos_output = xf;
+            vel_output = vf;
+            acc_output = af;
+            return Result::Finished;
         }
 
-        output = f + t * (e + t * (d + t * (c + t * (b + t * a))));
-        return State::Working;
+        pos_output = f + t * (e + t * (d + t * (c + t * (b + a * t))));
+        vel_output = e + t * (2 * d + t * (3 * c + t * (4 * b + 5 * a * t)));
+        acc_output = 2 * d + t * (6 * c + t * (12 * b + t * (20 * a)));
+        return Result::Working;
     }
 };
 

diff --git a/include/frankx/reaction.hpp b/include/frankx/reaction.hpp
@@ -19,8 +19,12 @@ enum class Measure {
 };
 
 enum class Comparison {
+  Equal,
+  NotEqual,
   Greater,
-  Smaller,
+  Less,
+  GreaterEqual,
+  LessEqual,
 };
 
 
@@ -41,6 +45,25 @@ struct Reaction {
   explicit Reaction(Measure measure, Comparison comparison, double value, tl::optional<std::function<WaypointMotion(const franka::RobotState&, double)>> action);
 
 private:
+  template<class T>
+  static inline bool compare(Comparison comparison, T a, T b) {
+    switch (comparison) {
+      default:
+      case Comparison::Equal:
+        return a == b;
+      case Comparison::NotEqual:
+        return a != b;
+      case Comparison::Greater:
+        return a > b;
+      case Comparison::Less:
+        return a < b;
+      case Comparison::GreaterEqual:
+        return a >= b;
+      case Comparison::LessEqual:
+        return a <= b;
+    }
+  }
+
   void setConditionCallback(Measure measure, Comparison comparison, double value);
 };
 

diff --git a/include/frankx/waypoint.hpp b/include/frankx/waypoint.hpp
@@ -1,6 +1,5 @@
 #pragma once
 
-
 #include <frankx/affine.hpp>
 #include <frankx/utils.hpp>
 #include <tl/optional.hpp>

diff --git a/src/python.cpp b/src/python.cpp
@@ -49,8 +49,12 @@ PYBIND11_MODULE(_frankx, m) {
         .export_values();
 
     py::enum_<Comparison>(m, "Comparison")
+        .value("Equal", Comparison::Equal)
+        .value("NotEqual", Comparison::NotEqual)
         .value("Greater", Comparison::Greater)
-        .value("Smaller", Comparison::Smaller)
+        .value("Less", Comparison::Less)
+        .value("GreaterEqual", Comparison::GreaterEqual)
+        .value("LessEqual", Comparison::LessEqual)
         .export_values();
 
     py::class_<Reaction>(m, "Reaction")

diff --git a/src/reaction.cpp b/src/reaction.cpp
@@ -25,47 +25,26 @@ Reaction::Reaction(Measure measure, Comparison comparison, double value, tl::opt
 void Reaction::setConditionCallback(Measure measure, Comparison comparison, double value) {
     switch (measure) {
         case Measure::ForceZ: {
-            if (comparison == Comparison::Smaller) {
-                condition_callback = [value](const franka::RobotState& robot_state, double time) {
-                    double force = robot_state.O_F_ext_hat_K[2];
-                    return (force < value);
-                };
-            } else if (comparison == Comparison::Greater) {
-                condition_callback = [value](const franka::RobotState& robot_state, double time) {
-                    double force = robot_state.O_F_ext_hat_K[2];
-                    return (force > value);
-                };
-            }
+            condition_callback = [value, comparison](const franka::RobotState& robot_state, double time) {
+                double force = robot_state.O_F_ext_hat_K[2];
+                return compare(comparison, force, value);
+            };
         } break;
         case Measure::ForceXYNorm: {
-            if (comparison == Comparison::Smaller) {
-                condition_callback = [value](const franka::RobotState& robot_state, double time) {
-                    double force = std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2);
-                    return (force < value);
-                };
-            } else if (comparison == Comparison::Greater) {
-                condition_callback = [value](const franka::RobotState& robot_state, double time) {
-                    double force = std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2);
-                    return (force > value);
-                };
-            }
+            condition_callback = [value, comparison](const franka::RobotState& robot_state, double time) {
+                double force = std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2);
+                return compare(comparison, force, value);
+            };
         } break;
         case Measure::ForceXYZNorm: {
-            if (comparison == Comparison::Smaller) {
-                condition_callback = [value](const franka::RobotState& robot_state, double time) {
-                    double force = std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2) + std::pow(robot_state.O_F_ext_hat_K[2], 2);
-                    return (force < value);
-                };
-            } else if (comparison == Comparison::Greater) {
-                condition_callback = [value](const franka::RobotState& robot_state, double time) {
-                    double force = std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2) + std::pow(robot_state.O_F_ext_hat_K[2], 2);
-                    return (force > value);
-                };
-            }
+            condition_callback = [value, comparison](const franka::RobotState& robot_state, double time) {
+                double force = std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2) + std::pow(robot_state.O_F_ext_hat_K[2], 2);
+                return compare(comparison, force, value);
+            };
         } break;
         case Measure::Time: {
-            condition_callback = [value](const franka::RobotState& robot_state, double time) {
-                return (time > value);
+            condition_callback = [value, comparison](const franka::RobotState& robot_state, double time) {
+                return compare(comparison, time, value);
             };
         }
     }

diff --git a/src/robot.cpp b/src/robot.cpp
@@ -177,23 +177,6 @@ bool Robot::move(const Affine& frame, WaypointMotion motion, MotionData& data) {
             for (int i = 0; i < steps; i++) {
                 result_value = rml->RMLPosition(*input_parameters, output_parameters.get(), flags);
 
-                // std::cout << "---" << std::endl;
-
-                // std::cout << input_parameters->TargetPositionVector << std::endl;
-                // std::cout << input_parameters->TargetVelocityVector << std::endl;
-
-                // std::cout << input_parameters->CurrentPositionVector << std::endl;
-                // std::cout << input_parameters->CurrentVelocityVector << std::endl;
-                // std::cout << input_parameters->CurrentAccelerationVector << std::endl;
-
-                // std::cout << input_parameters->MaxVelocityVector << std::endl;
-                // std::cout << input_parameters->MaxAccelerationVector << std::endl;
-                // std::cout << input_parameters->MaxJerkVector << std::endl;
-
-                // std::cout << output_parameters->NewPositionVector << std::endl;
-                // std::cout << output_parameters->NewVelocityVector << std::endl;
-                // std::cout << output_parameters->NewAccelerationVector << std::endl;
-
                 if (current_motion.reload || result_value == ReflexxesAPI::RML_FINAL_STATE_REACHED) {
                     waypoint_iterator += 1;
 
@@ -219,17 +202,6 @@ bool Robot::move(const Affine& frame, WaypointMotion motion, MotionData& data) {
 
                 } else if (result_value == ReflexxesAPI::RML_ERROR_INVALID_INPUT_VALUES) {
                     std::cout << "Invalid inputs:" << std::endl;
-                    // std::cout << input_parameters->TargetPositionVector << std::endl;
-                    // std::cout << input_parameters->TargetVelocityVector << std::endl;
-
-                    // std::cout << input_parameters->CurrentPositionVector << std::endl;
-                    // std::cout << input_parameters->CurrentVelocityVector << std::endl;
-                    // std::cout << input_parameters->CurrentAccelerationVector << std::endl;
-
-                    // std::cout << input_parameters->MaxVelocityVector << std::endl;
-                    // std::cout << input_parameters->MaxAccelerationVector << std::endl;
-                    // std::cout << input_parameters->MaxJerkVector << std::endl;
-
                     return franka::MotionFinished(CartesianPose(input_parameters->CurrentPositionVector, waypoint_has_elbow));
                 }
 
@@ -242,7 +214,6 @@ bool Robot::move(const Affine& frame, WaypointMotion motion, MotionData& data) {
         }, controller_mode);
 
     } catch (franka::Exception exception) {
-        // automaticErrorRecovery();
         std::cout << exception.what() << std::endl;
         return false;
     }

diff --git a/test/py-test.py b/test/py-test.py