added files

firmware/MotionControllerRP/src/motion_control/motion_controller.cpp

@@ -0,0 +1,40 @@
+
+//*** INCLUDE ***************************************************************************
+
+#include "motion_controller.h"
+#include "path_planner.h"
+#include "utilities/logging.h"
+
+//*** CLASS *****************************************************************************
+
+MotionController::MotionController(PathPlanner* path_planner) {
+  MotionController::path_planner = path_planner;
+  current_time = 0.0f;
+}
+
+bool MotionController::update(float dt, float* joint_positions, float* joint_velocities) {
+  // increment time counter
+  current_time += dt;
+
+  // check if end of current path segment exceeded and if so, fetch next one
+  float segment_duration = current_path_segment.get_duration();
+  while(current_time > segment_duration) {
+    // get next path segment from queue
+    bool queue_empty = !path_planner->pop_js_path_segment(current_path_segment);
+    if(queue_empty) {
+      current_time = segment_duration;
+      break;
+    }
+    
+    // update current time and segment duration
+    current_time -= segment_duration;
+    segment_duration = current_path_segment.get_duration();
+  }
+
+  if(!current_path_segment.is_initialized())
+    return false;
+
+  // evaluate path segment
+  current_path_segment.evaluate(current_time, joint_positions, joint_velocities);
+  return true;
+}

firmware/MotionControllerRP/src/motion_control/motion_controller.h

@@ -0,0 +1,26 @@
+#pragma once
+
+//*** INCLUDE ***************************************************************************
+
+#include "path_segment.h"
+
+//*** CLASS *****************************************************************************
+
+class PathPlanner;
+
+//--- MotionController ------------------------------------------------------------------
+
+class MotionController {
+  public:
+    MotionController(PathPlanner* path_planner);
+
+    // updates the motion controller and computes new joint positions and velocities
+    // after dt has passed. Ouput array must hav space for 'NUM_JOINTS' entries.
+    bool update(float dt, float* joint_positions, float* joint_velocities);
+
+  private:
+    PathPlanner* path_planner;
+
+    float current_time;
+    JointSpacePathSegment current_path_segment;
+};

firmware/MotionControllerRP/src/motion_control/path_planner.cpp

@@ -0,0 +1,101 @@
+
+#include "hardware/sync.h"
+
+#include "path_planner.h"
+#include "utilities/logging.h"
+
+PathPlanner::PathPlanner(IKinemtaicModel* kinematic_model, float time_step) {
+  segment_time_step = time_step;
+  kinematic_model = kinematic_model;
+}
+
+PathPlanner::~PathPlanner() {
+}
+
+void PathPlanner::set_kinematic_model(IKinemtaicModel* kinematic_model) {
+  PathPlanner::kinematic_model = kinematic_model;
+}
+
+bool PathPlanner::add_cartesian_path_segment(const CartesianPathSegment& path_segment) {
+  auto* new_segment = ct_path_segment_queue.push(path_segment);
+  if(new_segment == nullptr) {
+    // queue full
+    return false;
+  }
+
+  // TODO: do look ahead planning of queue
+  new_segment->compute_motion_profile(); // for testing
+
+  return true;
+}
+
+void PathPlanner::process(bool disable_interrupts_for_queue_update) {
+  // create new segment generator for next cartesian path segment
+  // the segment stays in the queue until it is completed
+  if(segment_generator == nullptr && ct_path_segment_queue.empty() == false) {
+    auto* current_segment = ct_path_segment_queue.peek();
+    segment_generator = new JointSpacePathSegmentGenerator(current_segment, 
+                                                           kinematic_model,
+                                                           segment_time_step);
+    /*LOG_INFO("Starting segment: duration=%fs, (%f, %f, %f)->(%f, %f, %f) | queue size: %i", 
+      current_segment->get_duration(),
+      current_segment->start_pose.translation.x,
+      current_segment->start_pose.translation.y,
+      current_segment->start_pose.translation.z,
+      current_segment->end_pose.translation.x,
+      current_segment->end_pose.translation.y,
+      current_segment->end_pose.translation.z,
+      ct_path_segment_queue.size()); */
+  }
+
+  // generate joint space segment
+  if(segment_generator != nullptr && js_path_segment_queue.full() == false) {
+    JointSpacePathSegment segment;
+    bool end_reached = segment_generator->generate_next(segment);
+
+    // update output queue
+    if(disable_interrupts_for_queue_update) {
+      uint32_t status = save_and_disable_interrupts();
+      js_path_segment_queue.push(segment);
+      restore_interrupts(status); 
+    } else {
+      js_path_segment_queue.push(segment);
+    }
+
+   // LOG_INFO("Adding joint space segment: [%f, %f, %f] -> [%f, %f, %f]", 
+   //   segment.start_pos[0], segment.start_pos[1],  segment.start_pos[2],
+   //   segment.end_pos[0], segment.end_pos[1],  segment.end_pos[2]);
+
+    // check if current cartesian path segmetn is completed
+    if(end_reached) {
+      // remove current cartesian path segment from ringbuffer
+      ct_path_segment_queue.pop();
+      // destroy segment generator
+      delete segment_generator;
+      segment_generator = nullptr;
+    }
+  }
+}
+
+/**
+ * retrieve 
+ */
+bool PathPlanner::pop_js_path_segment(JointSpacePathSegment& segment) {
+  return js_path_segment_queue.pop(segment);
+}
+
+bool PathPlanner::all_finished() {
+  return js_path_segment_queue.empty() && ct_path_segment_queue.empty() && segment_generator == nullptr;
+}
+
+int PathPlanner::input_queue_full() {
+  return ct_path_segment_queue.full();
+}
+
+int PathPlanner::input_queue_size() {
+  return ct_path_segment_queue.size();
+}
+
+void PathPlanner::run_look_ahead_planning() {
+
+}

firmware/MotionControllerRP/src/motion_control/path_planner.h

@@ -0,0 +1,57 @@
+#pragma once
+
+//*** INCLUDE ***************************************************************************
+
+#include "path_segment.h"
+#include "utilities/ringbuffer.h"
+
+//*** CLASS *****************************************************************************
+
+class IKinemtaicModel;
+
+//--- PathPlanner -----------------------------------------------------------------------
+
+class PathPlanner {
+  public:
+    static constexpr int CT_QUEUE_SIZE = 64;
+    static constexpr int JS_QUEUE_SIZE = 32;
+    
+  public:
+    PathPlanner(IKinemtaicModel* kinematic_model, float time_step);
+    ~PathPlanner();
+
+    // sets the kinematic model for foreward and inverse kinematic calculations
+    void set_kinematic_model(IKinemtaicModel* kinematic_model);
+
+    // adds a new cartesian space path segment to the planner queue
+    bool add_cartesian_path_segment(const CartesianPathSegment& path_segment);
+
+    // Retrieves the next joint space path segment from the queue, returns false
+    // if queue is empty.
+    bool pop_js_path_segment(JointSpacePathSegment& segment);
+
+    // Processes the queued cartesian path segments and generates one 
+    // joint space path segment if possible. Call this repeatedly.
+    void process(bool disable_interrupts_for_queue_update);
+
+    // returns true if all ques are empty and if everything is finished
+    bool all_finished();
+
+    // returns the number of free items in the input queue
+    int input_queue_full();
+
+    // returns the current number of queued items
+    int input_queue_size();
+
+  private:
+    void run_look_ahead_planning();
+
+  private:
+    RingBuffer<CartesianPathSegment, CT_QUEUE_SIZE> ct_path_segment_queue;
+    RingBuffer<JointSpacePathSegment, JS_QUEUE_SIZE> js_path_segment_queue;
+
+    IKinemtaicModel* kinematic_model;
+    JointSpacePathSegmentGenerator* segment_generator = nullptr;
+    float segment_time_step;
+};
+

firmware/MotionControllerRP/src/motion_control/path_segment.cpp

@@ -0,0 +1,282 @@
+#include "path_segment.h"
+#include "utilities/logging.h"
+#include "kinemtaic_models/kinematic_model_base.h"
+
+//--- MotionProfileConstAcc -------------------------------------------------------------
+
+MotionProfileConstAcc::MotionProfileConstAcc(float dwell_time) {
+  MotionProfileConstAcc::t1 = 0.0f;
+  MotionProfileConstAcc::t2 = dwell_time;
+  MotionProfileConstAcc::t3 = dwell_time;
+  MotionProfileConstAcc::d1 = 0.0f;
+  MotionProfileConstAcc::d2 = 1.0f;
+  MotionProfileConstAcc::v_peak = 0.0f;
+  MotionProfileConstAcc::acceleration = 0.0f;
+}
+
+MotionProfileConstAcc::MotionProfileConstAcc(
+    float distance,
+    float v_start,
+    float v_end,
+    float max_velocity,
+    float max_acceleration)
+{
+  MotionProfileConstAcc::acceleration = max_acceleration;
+  MotionProfileConstAcc::v_start = v_start;
+  MotionProfileConstAcc::v_end = v_end;
+
+  if (distance <= 1e-7f) {
+    MotionProfileConstAcc::t1 = 0.0f;
+    MotionProfileConstAcc::t2 = 0.0f;
+    MotionProfileConstAcc::t3 = 0.0f;
+    MotionProfileConstAcc::d1 = 0.0f;
+    MotionProfileConstAcc::d2 = 1.0f;
+    MotionProfileConstAcc::v_peak = 0.0f;
+    MotionProfileConstAcc::acceleration = 0.0f;
+  } else {
+    const float inv_max_acceleration = 1.0f / max_acceleration;
+
+    // Time to accelerate/decelerate, using multiplication by inverse accel
+    float t_accel = (max_velocity - v_start) * inv_max_acceleration;
+    float t_decel = (max_velocity - v_end) * inv_max_acceleration;
+
+    // Distances covered during accel/decel
+    float d_accel = 0.5f * (v_start + max_velocity) * t_accel;
+    float d_decel = 0.5f * (max_velocity + v_end) * t_decel;
+
+    float d_cruise = distance - (d_accel + d_decel);
+
+    if (d_cruise >= 0.0f) {
+      // Trapezoidal velocity profile
+      MotionProfileConstAcc::t1 = t_accel;
+      MotionProfileConstAcc::t2 = t1 + d_cruise / max_velocity;
+      MotionProfileConstAcc::t3 = t2 + t_decel;
+      MotionProfileConstAcc::v_peak = max_velocity;
+    } else {
+      // Triangular velocity profile: recompute peak velocity v_peak
+      float v_peak_sq = max_acceleration * distance + 0.5f * (v_start * v_start + v_end * v_end);
+      float v_peak = std::sqrt(std::max(0.0f, v_peak_sq));
+
+      MotionProfileConstAcc::t1 = (v_peak - v_start) * inv_max_acceleration;
+      MotionProfileConstAcc::t2 = t1 + 0.0f;
+      MotionProfileConstAcc::t3 = t2 + (v_peak - v_end) * inv_max_acceleration;
+      MotionProfileConstAcc::v_peak = v_peak;
+    }
+  }
+
+  // normalize velocity and acceleration to interpolator range (0..1)
+  const float inv_distance = 1.0f/distance;
+  v_start *= inv_distance;
+  v_end *= inv_distance;
+  v_peak *= inv_distance;
+  acceleration *= inv_distance;
+
+  // precompute some values for faster evaluation
+  MotionProfileConstAcc::d1 = 0.5f * (v_start + v_peak) * t1;
+  MotionProfileConstAcc::d2 = d1 + v_peak * (t2-t1);
+
+  //LOG_INFO("d1=%f, d2=%f, d3=%f", d1, d2, distance);
+  //LOG_INFO("t1=%f, t2=%f, t3=%f", t1, t2, t3);
+}
+
+float MotionProfileConstAcc::evaluate(float time) const {
+  if (time <= 0.0f) {
+    return 0.0f;
+  } else if (time < t1) {
+    // Acceleration phase
+    return v_start * time + 0.5f * acceleration * time * time;
+  } else if (time < t2) {
+    // Cruise phase
+    float dt = time - t1;
+    return d1 + v_peak * dt;
+  } else if (time < t3) {
+    // Deceleration phase
+    float dt = time - t2;
+    return d2 + v_peak * dt - 0.5f * acceleration * dt * dt;
+  } else {
+    // Finished
+    return 1.0f;
+  }
+}
+
+//--- CartesianPathSegment --------------------------------------------------------------
+
+CartesianPathSegment::CartesianPathSegment() {
+  dwell_time = 0.0f;
+}
+
+CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
+                                           const Pose6DF& end_pose,
+                                           const LinearAngular& target_velocity,
+                                           const LinearAngular& max_acceleration)
+{
+  CartesianPathSegment::dwell_time = 0.0f;
+  CartesianPathSegment::start_pose = start_pose;
+  CartesianPathSegment::end_pose = end_pose;
+
+  CartesianPathSegment::target_velocity = target_velocity;
+  CartesianPathSegment::start_velocity = LinearAngular(0.0f, 0.0f);
+  CartesianPathSegment::end_velocity = LinearAngular(0.0f, 0.0f);
+  CartesianPathSegment::max_acceleration = max_acceleration;
+
+  travel_distance.linear = (end_pose.translation - start_pose.translation).length();
+  travel_distance.angular = (start_pose.rotation.normalized_inverse() * end_pose.rotation).angle();
+}
+
+CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
+{
+  CartesianPathSegment::dwell_time = dwell_time;
+  CartesianPathSegment::start_pose = pose;
+  CartesianPathSegment::end_pose = pose;
+
+  CartesianPathSegment::target_velocity = 0.0f;
+  CartesianPathSegment::start_velocity = LinearAngular(0.0f, 0.0f);
+  CartesianPathSegment::end_velocity = LinearAngular(0.0f, 0.0f);
+  CartesianPathSegment::max_acceleration = 0.0f;
+
+  travel_distance.linear = 0.0f;
+  travel_distance.angular = 0.0f;
+}
+
+
+void CartesianPathSegment::compute_motion_profile() {
+  // LOG_INFO("compute_motion_profile...");
+  if(dwell_time > 0.0f) {
+    motion_profile = MotionProfileConstAcc(dwell_time);
+  } else {
+    MotionProfileConstAcc linear_profile(travel_distance.linear,start_velocity.linear, 
+                                        end_velocity.linear, target_velocity.linear, 
+                                        max_acceleration.linear);
+
+    MotionProfileConstAcc angular_profile(travel_distance.angular, start_velocity.angular, 
+                                          end_velocity.angular, target_velocity.angular, 
+                                          max_acceleration.angular);
+
+    // select profile that requires the longest time
+    if(linear_profile.t3 > angular_profile.t3) {
+      motion_profile = linear_profile;
+    } else {
+      motion_profile = angular_profile;
+    }
+  }
+}
+
+void CartesianPathSegment::evaluate(float time, Pose6DF& pose) const {
+  // evaluate motion profile
+  float t = motion_profile.evaluate(time);
+
+  // interpolate pose
+  pose = Pose6DF::lerp(start_pose, end_pose, t);
+}
+
+float CartesianPathSegment::get_duration() const {
+  return motion_profile.t3;
+}
+
+//--- JointSpacePathSegment -------------------------------------------------------------
+
+JointSpacePathSegment::JointSpacePathSegment() {
+  for(int i=0; i<NUM_JOINTS; i++) {
+    JointSpacePathSegment::start_pos[i] = 0.0f;
+    JointSpacePathSegment::end_pos[i] = 0.0f;
+  }
+  duration = 0.0f;
+  inv_duration = 0.0f;
+  initialized = false;
+}
+
+JointSpacePathSegment::JointSpacePathSegment(
+  const float start_pos[NUM_JOINTS],
+  const float end_pos[NUM_JOINTS],
+  float duration)
+{
+  JointSpacePathSegment::duration = duration;
+  JointSpacePathSegment::inv_duration = 1.0f/std::max(duration, 1e-7f);
+
+  for(int i=0; i<NUM_JOINTS; i++) {
+    JointSpacePathSegment::start_pos[i] = start_pos[i];
+    JointSpacePathSegment::end_pos[i] = end_pos[i];
+  }
+
+  initialized = true;
+}
+
+void JointSpacePathSegment::evaluate(
+  float time, 
+  float joint_positions[NUM_JOINTS], 
+  float joint_velocity[NUM_JOINTS]) const
+{
+  float t = time*inv_duration;
+  float s = 1.0f-t;
+
+  for(int i=0; i<NUM_JOINTS; i++) {
+    joint_positions[i] = start_pos[i]*s + end_pos[i]*t;
+    joint_velocity[i] = 0;
+  }
+}
+
+float JointSpacePathSegment::get_duration() {
+  return duration;
+}
+
+bool JointSpacePathSegment::is_initialized() {
+  return initialized;
+}
+
+//--- JointSpacePathSegmentGenerator ----------------------------------------------------
+
+JointSpacePathSegmentGenerator::JointSpacePathSegmentGenerator(
+  const CartesianPathSegment* path_segment,
+  IKinemtaicModel* kinematic_model,
+  float time_step)
+{
+  JointSpacePathSegmentGenerator::path_segment = path_segment;
+  JointSpacePathSegmentGenerator::kinematic_model = kinematic_model;
+  current_time = 0.0f;
+  delta_time = time_step;
+  end_time = path_segment->get_duration();
+  end_time_with_eps = end_time-0.2f*delta_time;
+
+  // check kinematic model
+  if(kinematic_model->get_joint_count() != NUM_JOINTS) {
+    LOG_ERROR("NUM_JOINTS (%i) differs from value required by Kinematic model (%i)",
+      NUM_JOINTS, kinematic_model->get_joint_count());
+    error_trap("Fatal Error");
+  }
+
+  // evaluate inverse kinematic model to et start joint positions
+  kinematic_model->inverse(path_segment->start_pose, current_joint_pos);
+}
+
+bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_path_segment) {
+  bool end_reached = false;
+
+  // increment evaluation position
+  float initial_time = current_time;
+  current_time += delta_time;
+
+  // check if end of path is reached, check against end_t which includes an epsilon
+  // to prevent tiny segments at path end (snaps to t=1.0 within tolerance).
+  if(current_time >= end_time_with_eps) {
+    end_reached = true;
+    current_time = end_time;  // snap to 1.0
+  }
+
+  // evaluate path to get new end position
+  Pose6DF seg_end_pose;
+  path_segment->evaluate(current_time, seg_end_pose);
+
+  // evaluate inverse kinematic model here
+  float next_joint_pos[NUM_JOINTS];
+  kinematic_model->inverse(seg_end_pose, next_joint_pos);
+  
+  // create joint space path segment
+  float duration = current_time-initial_time;
+  js_path_segment = JointSpacePathSegment(current_joint_pos, next_joint_pos, duration);
+
+  // update current joint pos
+  for(int i=0; i<NUM_JOINTS; i++)
+    current_joint_pos[i] = next_joint_pos[i];
+
+  return end_reached;
+}

firmware/MotionControllerRP/src/motion_control/path_segment.h

@@ -0,0 +1,137 @@
+#pragma once
+
+//*** INCLUDE ***************************************************************************
+
+#include "utilities/math3d.h"
+
+//*** CONST *****************************************************************************
+
+constexpr int NUM_JOINTS = 3;
+
+//*** CLASS *****************************************************************************
+
+class IKinemtaicModel;
+
+//--- JointInfo -------------------------------------------------------------------------
+
+class JointInfo {
+  public:
+    float max_velocity;
+    float max_acceleration;
+};
+
+//--- MotionProfileConstAcc ------------------------------------------------------------
+
+class MotionProfileConstAcc {
+  public: 
+    MotionProfileConstAcc() = default;
+    MotionProfileConstAcc(float distance,
+                          float v_start,
+                          float v_end,
+                          float max_velocity,
+                          float max_acceleration);
+    MotionProfileConstAcc(float dwell_time);
+
+    // returns an interpolator value in range [0..1] that can be used to interpolate
+    // start and end poses
+    float evaluate(float time) const;
+
+  public:
+    float t1 = 0.0f;  // end time of accelleration phase
+    float t2 = 0.0f;  // end time of cruise phase
+    float t3 = 0.0f;  // end time of decellartion phase (total time)
+    float acceleration = 1.0f;
+
+    float v_start;
+    float v_end;
+
+    float v_peak;
+    float d1;         // distance after accelleration phase
+    float d2;         // distance after cruise phase
+};
+
+//--- CartesianPathSegment --------------------------------------------------------------
+
+// A Linear motion path segment in 6DOF Cartesian Space
+class CartesianPathSegment {
+  public:
+    CartesianPathSegment();
+    CartesianPathSegment(const Pose6DF& start_pose,
+                         const Pose6DF& end_pose,
+                         const LinearAngular& velocity,
+                         const LinearAngular& max_acceleration);
+    CartesianPathSegment(const Pose6DF& pose,float dwell_time);
+
+    void evaluate(float time, Pose6DF& pose) const;
+    float get_duration() const;
+
+    void compute_motion_profile();
+
+  public:
+    Pose6DF start_pose;
+    Pose6DF end_pose;
+
+    LinearAngular start_velocity;
+    LinearAngular target_velocity;
+    LinearAngular end_velocity;
+    LinearAngular max_acceleration;
+
+    LinearAngular travel_distance;
+    MotionProfileConstAcc motion_profile;
+
+    float dwell_time;  // stay at start position for given duration if dwell_time > 0
+};
+
+//--- JointSpacePathSegment -------------------------------------------------------------
+
+// A linear motion path segment in Joint Space
+class JointSpacePathSegment {
+  public:
+    JointSpacePathSegment();
+    JointSpacePathSegment(const float start_pos[NUM_JOINTS],
+                          const float end_pos[NUM_JOINTS],
+                          const float duration);
+
+    void evaluate(float time, 
+                  float joint_positions[NUM_JOINTS], 
+                  float joint_velocity[NUM_JOINTS]) const;
+
+    float get_duration();
+
+    bool is_initialized();
+
+  public:
+    bool initialized;
+    float start_pos[NUM_JOINTS];
+    float end_pos[NUM_JOINTS];
+    float start_velocity[NUM_JOINTS];
+    float end_velocity[NUM_JOINTS];
+
+    float duration;
+    float inv_duration;
+};
+
+//--- JointSpacePathSegmentGenerator ----------------------------------------------------
+
+class JointSpacePathSegmentGenerator {
+  public:
+    JointSpacePathSegmentGenerator(
+      const CartesianPathSegment* path_segment,
+      IKinemtaicModel* kinematic_model,
+      float time_step
+    );
+
+    void reset();
+    bool generate_next(JointSpacePathSegment& js_path_segment);
+
+  private:
+    float delta_time;                     // time step size
+    float current_time;                   // current t in range [0..1]
+    float end_time;                       // end time
+    float end_time_with_eps;              // end time including a small negative epsilon
+    float current_joint_pos[NUM_JOINTS];  // current joint positions
+
+    const CartesianPathSegment* path_segment = nullptr;
+    IKinemtaicModel* kinematic_model;
+};
+