fafo/OpenMicroManipulator
17
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

updated firmware

Browse source
This commit is contained in:
0x23 2025-08-28 14:07:54 +02:00
parent 9891b8ed44
commit f17ca0c820
11 changed files with 273 additions and 56 deletions
Download patch file Download diff file Expand all files Collapse all files

120
firmware/MotionControllerRP/src/motion_control/path_planner.cpp
View file

@ -4,9 +4,14 @@
#include "path_planner.h"
#include "utilities/logging.h"
#include <algorithm>
PathPlanner::PathPlanner(IKinemtaicModel* kinematic_model, float time_step) {
segment_time_step = time_step;
kinematic_model = kinematic_model;
junction_deviation.linear = 0.0001; // mm
junction_deviation.angular = 0.001; // rad
}
PathPlanner::~PathPlanner() {
@ -23,9 +28,6 @@ bool PathPlanner::add_cartesian_path_segment(const CartesianPathSegment& path_se
return false;
}
// TODO: do look ahead planning of queue
new_segment->compute_motion_profile(); // for testing
return true;
}
@ -34,9 +36,15 @@ void PathPlanner::process(bool disable_interrupts_for_queue_update) {
// 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();
// compute motion profile ( active segment can not change anymore )
current_segment->compute_motion_profile();
// create path segment generator
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,
@ -97,5 +105,111 @@ int PathPlanner::input_queue_size() {
}
void PathPlanner::run_look_ahead_planning() {
if(ct_path_segment_queue.empty())
return;
int n = ct_path_segment_queue.size();
auto* last_segment = ct_path_segment_queue.get(n - 1);
if(segment_generator != nullptr && last_segment == segment_generator->get_path_segment())
return;
// --- Reverse pass ---
// Start from the last segment, set its end velocity to zero (or target)
last_segment->end_velocity = LinearAngular{0, 0};
// Propagate backward the feasible start velocities
for (int i = n - 2; i >= 0; i--) {
CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
// dont change active segment
if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
s2->start_velocity = s1->end_velocity;
continue;
}
// Get minimum acceleration capability at junction
float acc = std::min(s1->max_acceleration.linear, s2->max_acceleration.linear);
// Compute max junction velocity feasible at junction between s1 and s2
float max_junction_velocity_linear = compute_max_junction_velocity(
s1->translation_delta_normalized,
s2->translation_delta_normalized,
acc,
junction_deviation.linear
);
// no angular junction velocity limit yet
float max_junction_velocity_angular = 1e10;
// compute max velocity delta
float v_start_max_linear = std::sqrt(std::max(0.0f, powf(s1->end_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
float v_start_max_angular = std::sqrt(std::max(0.0f, powf(s1->end_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
// compute final junction velocity
LinearAngular junction_velocity;
junction_velocity.linear = std::min(max_junction_velocity_linear, std::min(s2->target_velocity.linear, v_start_max_linear));
junction_velocity.angular = std::min(max_junction_velocity_angular, std::min(s2->target_velocity.angular, v_start_max_angular));
// set calculated velocity to both path segments
s1->end_velocity = junction_velocity;
s2->start_velocity = junction_velocity;
}
// --- Forward pass ---
for (int i = 0; i < n - 1; i++) {
CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
// dont change active segment
if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
s2->start_velocity = s1->end_velocity;
continue;
}
float v_start = s1->start_velocity.linear;
float v_end_max_linear = std::sqrt(std::max(0.0f, powf(s1->start_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
float v_end_max_angular = std::sqrt(std::max(0.0f, powf(s1->start_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
LinearAngular junction_velocity;
if(s1->end_velocity.linear > v_end_max_linear) s1->end_velocity.linear = v_end_max_linear;
if(s1->end_velocity.angular > v_end_max_angular) s1->end_velocity.angular = v_end_max_angular;
s2->start_velocity = s1->end_velocity;
}
// debug
// print_cartesian_path_segments();
}
float PathPlanner::compute_max_junction_velocity(const Vec3F& dir_in_normalized, const Vec3F& dir_out_normalized, float acceleration, float junction_deviation) {
const float EPSILON = 1e-6f;
const float COS_NEAR_STRAIGHT = 0.9999f;
const float COS_NEAR_OPPOSITE = -0.9999f;
// Compute the cosine of the angle between the directions (negative dot product)
float cos_theta = -dir_in_normalized.dot(dir_out_normalized);
// Compute sin(θ/2) using half-angle identity: sin²(θ/2) = (1 - cosθ) / 2
float sin_theta_d2 = std::sqrt(std::max(0.0f, 0.5f * (1.0f - cos_theta)));
// Compute vmax using classical junction deviation formula
float denom = std::max(1.0f - sin_theta_d2, EPSILON);
float vmax = std::sqrt(acceleration * junction_deviation * sin_theta_d2 / denom);
return vmax;
}
void PathPlanner::print_cartesian_path_segments() {
LOG_INFO("Cartesian Path Segment Info");
int n = ct_path_segment_queue.size();
for (int i = 0; i < n; i++) {
CartesianPathSegment* s = ct_path_segment_queue.get(i);
LOG_INFO(" Segment %02i: [%f, %f, %f]->[%f, %f, %f] l=%f vs=%fmm/s ve=%fmm/s", i,
s->start_pose.translation.x, s->start_pose.translation.y, s->start_pose.translation.z,
s->end_pose.translation.x, s->end_pose.translation.y, s->end_pose.translation.z,
s->travel_distance.linear,
s->start_velocity.linear, s->end_velocity.linear);
}
}

15
firmware/MotionControllerRP/src/motion_control/path_planner.h
View file

@ -13,7 +13,7 @@ class IKinemtaicModel;
class PathPlanner {
public:
static constexpr int CT_QUEUE_SIZE = 64;
static constexpr int CT_QUEUE_SIZE = 32;
static constexpr int JS_QUEUE_SIZE = 32;
public:
@ -26,6 +26,10 @@ class PathPlanner {
// adds a new cartesian space path segment to the planner queue
bool add_cartesian_path_segment(const CartesianPathSegment& path_segment);
// runs look ahead path planning. call this everytime after one or more cartesian
// path segments have been added
void run_look_ahead_planning();
// Retrieves the next joint space path segment from the queue, returns false
// if queue is empty.
bool pop_js_path_segment(JointSpacePathSegment& segment);
@ -44,7 +48,13 @@ class PathPlanner {
int input_queue_size();
private:
void run_look_ahead_planning();
float compute_max_junction_velocity(
const Vec3F& dir_in_normalized,
const Vec3F& dir_out_normalized,
float acceleration,
float junction_deviation);
void print_cartesian_path_segments();
private:
RingBuffer<CartesianPathSegment, CT_QUEUE_SIZE> ct_path_segment_queue;
@ -53,5 +63,6 @@ class PathPlanner {
IKinemtaicModel* kinematic_model;
JointSpacePathSegmentGenerator* segment_generator = nullptr;
float segment_time_step;
LinearAngular junction_deviation;
};

71
firmware/MotionControllerRP/src/motion_control/path_segment.cpp
View file

@ -21,17 +21,15 @@ MotionProfileConstAcc::MotionProfileConstAcc(
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_start = 0.0;
MotionProfileConstAcc::v_peak = 0.0f;
MotionProfileConstAcc::v_end = 0.0;
MotionProfileConstAcc::acceleration = 0.0f;
} else {
const float inv_max_acceleration = 1.0f / max_acceleration;
@ -43,40 +41,44 @@ MotionProfileConstAcc::MotionProfileConstAcc(
// 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);
float v_peak = 0.0;
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;
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));
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;
max_acceleration *= inv_distance;
v_start *= inv_distance;
v_end *= inv_distance;
v_peak *= inv_distance;
// assign values
MotionProfileConstAcc::d1 = 0.5f * (v_start + v_peak) * t1;
MotionProfileConstAcc::d2 = d1 + v_peak * (t2-t1);
MotionProfileConstAcc::v_start = v_start;
MotionProfileConstAcc::v_end = v_end;
MotionProfileConstAcc::v_peak = v_peak;
MotionProfileConstAcc::acceleration = max_acceleration;
}
// 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);
// LOG_INFO("d1=%f, d2=%f, d3=%f", MotionProfileConstAcc::d1, MotionProfileConstAcc::d2, distance);
// LOG_INFO("t1=%f, t2=%f, t3=%f", MotionProfileConstAcc::t1, MotionProfileConstAcc::t2, MotionProfileConstAcc::t3);
// LOG_INFO("vs=%f, vp=%f, ve=%f", MotionProfileConstAcc::v_start, MotionProfileConstAcc::v_peak, MotionProfileConstAcc::v_end);
}
float MotionProfileConstAcc::evaluate(float time) const {
@ -119,8 +121,19 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
CartesianPathSegment::end_velocity = LinearAngular(0.0f, 0.0f);
CartesianPathSegment::max_acceleration = max_acceleration;
travel_distance.linear = (end_pose.translation - start_pose.translation).length();
Vec3F translation_delta = end_pose.translation - start_pose.translation;
travel_distance.linear = (translation_delta).length();
travel_distance.angular = (start_pose.rotation.normalized_inverse() * end_pose.rotation).angle();
translation_delta_normalized = translation_delta.normalized();
/*
QuaternionF rotation_delta = (end_pose.rotation * start_pose.rotation.normalized_inverse());
Vec3F axis;
float angle;
rotation_delta.to_axis_angle(axis, angle);
rotation_delta_axis = axis; */
}
CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
@ -144,9 +157,9 @@ void CartesianPathSegment::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 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,
@ -164,6 +177,7 @@ void CartesianPathSegment::compute_motion_profile() {
void CartesianPathSegment::evaluate(float time, Pose6DF& pose) const {
// evaluate motion profile
float t = motion_profile.evaluate(time);
// LOG_INFO(">t_x [mm]: %f", t);
// interpolate pose
pose = Pose6DF::lerp(start_pose, end_pose, t);
@ -265,6 +279,7 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
// evaluate path to get new end position
Pose6DF seg_end_pose;
path_segment->evaluate(current_time, seg_end_pose);
// LOG_INFO(">pos_x [mm]: %f", seg_end_pose.translation.x);
// evaluate inverse kinematic model here
float next_joint_pos[NUM_JOINTS];
@ -279,4 +294,8 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
current_joint_pos[i] = next_joint_pos[i];
return end_reached;
}
}
const CartesianPathSegment* JointSpacePathSegmentGenerator::get_path_segment() const {
return path_segment;
}

10
firmware/MotionControllerRP/src/motion_control/path_segment.h
View file

@ -64,7 +64,6 @@ class CartesianPathSegment {
void evaluate(float time, Pose6DF& pose) const;
float get_duration() const;
void compute_motion_profile();
public:
@ -76,6 +75,10 @@ class CartesianPathSegment {
LinearAngular end_velocity;
LinearAngular max_acceleration;
LinearAngular max_velocity_delta;
Vec3F translation_delta_normalized;
//Vec3F rotation_delta_axis;
LinearAngular travel_distance;
MotionProfileConstAcc motion_profile;
@ -121,8 +124,9 @@ class JointSpacePathSegmentGenerator {
float time_step
);
void reset();
bool generate_next(JointSpacePathSegment& js_path_segment);
void reset();
bool generate_next(JointSpacePathSegment& js_path_segment);
const CartesianPathSegment* get_path_segment() const;
private:
float delta_time; // time step size