Skip to content

Commit acff411

Browse files
andypughandypugh
authored
Merge pull request #3802 from grandixximo/master
tp: integrate Ruckig S-curve trajectory planner for planner_type 1
2 parents e710c15 + 1931b85 commit acff411

97 files changed

Lines changed: 54241 additions & 1050 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎src/Makefile‎

Lines changed: 99 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -213,24 +213,24 @@ PROGRAMS :=
213213
# file names, but this is what allows 'make src.i' to produce proper
214214
# preprocessed source when src.c needs a customized compile flag.
215215
# See Submakefile.skel for an example.
216-
TOOBJS = $(patsubst %.cc,objects/%.o,$(patsubst %.c,objects/%.o,$(1)))
217-
TODEPS = $(patsubst %.cc,objects/%.d,$(patsubst %.c,objects/%.d,$(1)))
218-
TOOBJSDEPS = $(call TOOBJS,$(1)) $(call TODEPS, $(1)) $(patsubst %.cc,%.ii,$(patsubst %.c,%.i,$(1)))
216+
TOOBJS = $(patsubst %.cpp,objects/%.o,$(patsubst %.cc,objects/%.o,$(patsubst %.c,objects/%.o,$(1))))
217+
TODEPS = $(patsubst %.cpp,objects/%.d,$(patsubst %.cc,objects/%.d,$(patsubst %.c,objects/%.d,$(1))))
218+
TOOBJSDEPS = $(call TOOBJS,$(1)) $(call TODEPS, $(1)) $(patsubst %.cpp,%.ii,$(patsubst %.cc,%.ii,$(patsubst %.c,%.i,$(1))))
219219

220220
SUBMAKEFILES := $(patsubst %,%/Submakefile,$(SUBDIRS))
221221
-include $(wildcard $(SUBMAKEFILES))
222222

223223
# This checks that all the things listed in USERSRCS are either C files
224224
# or C++ files
225225
ASSERT_EMPTY = $(if $(1), $(error "Should be empty but is not: $(1)"))
226-
$(call ASSERT_EMPTY,$(filter-out %.c %.cc, $(USERSRCS)))
226+
$(call ASSERT_EMPTY,$(filter-out %.c %.cc %.cpp, $(USERSRCS)))
227227

228228
$(call TOOBJS,$(PYSRCS)) : EXTRAFLAGS += -fPIC -fno-strict-aliasing
229229
USERSRCS += $(PYSRCS)
230230

231231
# Find the list of object files for each type of source file
232232
CUSERSRCS := $(filter %.c,$(USERSRCS))
233-
CXXUSERSRCS := $(filter %.cc,$(USERSRCS))
233+
CXXUSERSRCS := $(filter %.cc %.cpp,$(USERSRCS))
234234
CUSEROBJS := $(call TOOBJS,$(CUSERSRCS))
235235
CXXUSEROBJS += $(call TOOBJS,$(CXXUSERSRCS))
236236

@@ -279,7 +279,35 @@ $(sort $(CUSEROBJS)) : objects/%.o: %.c
279279
-MP -MD -MF "${@:.o=.d}" -MT "$@" \
280280
$< -o $@
281281

282-
$(sort $(CXXUSEROBJS)) : objects/%.o: %.cc
282+
# Ruckig files need C++20
283+
RUCKIG_CXX_FILES := $(filter emc/tp/ruckig%,$(CXXUSERSRCS))
284+
NON_RUCKIG_CXX_FILES := $(filter-out emc/tp/ruckig%,$(CXXUSERSRCS))
285+
286+
$(sort $(patsubst %.cpp,objects/%.o,$(filter %.cpp,$(RUCKIG_CXX_FILES)))) : objects/%.o: %.cpp
287+
$(ECHO) Compiling $<
288+
@mkdir -p $(dir $@)
289+
@rm -f $@
290+
$(Q)$(CXX) -c $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 \
291+
-MP -MD -MF "${@:.o=.d}" -MT "$@" \
292+
$< -o $@
293+
294+
$(sort $(patsubst %.cc,objects/%.o,$(filter %.cc,$(RUCKIG_CXX_FILES)))) : objects/%.o: %.cc
295+
$(ECHO) Compiling $<
296+
@mkdir -p $(dir $@)
297+
@rm -f $@
298+
$(Q)$(CXX) -c $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 \
299+
-MP -MD -MF "${@:.o=.d}" -MT "$@" \
300+
$< -o $@
301+
302+
$(sort $(patsubst %.cpp,objects/%.o,$(filter %.cpp,$(NON_RUCKIG_CXX_FILES)))) : objects/%.o: %.cpp
303+
$(ECHO) Compiling $<
304+
@mkdir -p $(dir $@)
305+
@rm -f $@
306+
$(Q)$(CXX) -c $(CXXFLAGS) $(EXTRAFLAGS) \
307+
-MP -MD -MF "${@:.o=.d}" -MT "$@" \
308+
$< -o $@
309+
310+
$(sort $(patsubst %.cc,objects/%.o,$(filter %.cc,$(NON_RUCKIG_CXX_FILES)))) : objects/%.o: %.cc
283311
$(ECHO) Compiling $<
284312
@mkdir -p $(dir $@)
285313
@rm -f $@
@@ -292,7 +320,19 @@ $(sort $(patsubst %.c,%.i,$(CUSERSRCS))): %.i: %.c
292320
$(ECHO) Preprocessing $< to $@
293321
$(Q)$(CC) -dD $(CFLAGS) $(EXTRAFLAGS) -E $< -o $@
294322

295-
$(sort $(patsubst %.cc,%.ii,$(CXXUSERSRCS))): %.ii: %.cc
323+
$(sort $(patsubst %.cc,%.ii,$(filter %.cc,$(RUCKIG_CXX_FILES)))): %.ii: %.cc
324+
$(ECHO) Preprocessing $< to $@
325+
$(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 -E $< -o $@
326+
327+
$(sort $(patsubst %.cpp,%.ii,$(filter %.cpp,$(RUCKIG_CXX_FILES)))): %.ii: %.cpp
328+
$(ECHO) Preprocessing $< to $@
329+
$(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -std=c++20 -E $< -o $@
330+
331+
$(sort $(patsubst %.cc,%.ii,$(filter %.cc,$(NON_RUCKIG_CXX_FILES)))): %.ii: %.cc
332+
$(ECHO) Preprocessing $< to $@
333+
$(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -E $< -o $@
334+
335+
$(sort $(patsubst %.cpp,%.ii,$(filter %.cpp,$(NON_RUCKIG_CXX_FILES)))): %.ii: %.cpp
296336
$(ECHO) Preprocessing $< to $@
297337
$(Q)$(CXX) -dD $(CXXFLAGS) $(EXTRAFLAGS) -E $< -o $@
298338

@@ -884,7 +924,7 @@ endif
884924
# "kbuild" system. $(BASEPWD) is used here, instead of relative paths, because
885925
# that's what kbuild seems to require
886926

887-
EXTRA_CFLAGS := $(filter-out -ffast-math,$(RTFLAGS)) -D__MODULE__ -I$(BASEPWD)/../include -I$(BASEPWD) -I$(BASEPWD)/libnml/linklist \
927+
EXTRA_CFLAGS := $(filter-out -ffast-math,$(RTFLAGS)) -D__MODULE__ -I$(BASEPWD)/../include -I$(BASEPWD) -I$(BASEPWD)/libnml/linklist -I$(BASEPWD)/emc/tp/ruckig/include \
888928
-I$(BASEPWD)/libnml/cms -I$(BASEPWD)/libnml/rcs -I$(BASEPWD)/libnml/inifile \
889929
-I$(BASEPWD)/libnml/os_intf -I$(BASEPWD)/libnml/nml -I$(BASEPWD)/libnml/buffer \
890930
-I$(BASEPWD)/libnml/posemath -I$(BASEPWD)/rtapi -I$(BASEPWD)/hal \
@@ -1231,6 +1271,18 @@ motmod-objs += emc/motion/emcmotutil.o
12311271
motmod-objs += emc/motion/stashf.o
12321272
motmod-objs += emc/motion/dbuf.o
12331273
motmod-objs += emc/tp/sp_scurve.o
1274+
motmod-objs += emc/tp/ruckig_wrapper.o
1275+
motmod-objs += emc/tp/ruckig/src/ruckig/brake.o
1276+
motmod-objs += emc/tp/ruckig/src/ruckig/position_first_step1.o
1277+
motmod-objs += emc/tp/ruckig/src/ruckig/position_first_step2.o
1278+
motmod-objs += emc/tp/ruckig/src/ruckig/position_second_step1.o
1279+
motmod-objs += emc/tp/ruckig/src/ruckig/position_second_step2.o
1280+
motmod-objs += emc/tp/ruckig/src/ruckig/position_third_step1.o
1281+
motmod-objs += emc/tp/ruckig/src/ruckig/position_third_step2.o
1282+
motmod-objs += emc/tp/ruckig/src/ruckig/velocity_second_step1.o
1283+
motmod-objs += emc/tp/ruckig/src/ruckig/velocity_second_step2.o
1284+
motmod-objs += emc/tp/ruckig/src/ruckig/velocity_third_step1.o
1285+
motmod-objs += emc/tp/ruckig/src/ruckig/velocity_third_step2.o
12341286

12351287
obj-m += homemod.o
12361288
homemod-objs := emc/motion/homemod.o
@@ -1246,6 +1298,18 @@ tpmod-objs += emc/tp/blendmath.o
12461298
tpmod-objs += emc/nml_intf/emcpose.o
12471299
tpmod-objs += libnml/posemath/_posemath.o
12481300
tpmod-objs += emc/tp/sp_scurve.o
1301+
tpmod-objs += emc/tp/ruckig_wrapper.o
1302+
tpmod-objs += emc/tp/ruckig/src/ruckig/brake.o
1303+
tpmod-objs += emc/tp/ruckig/src/ruckig/position_first_step1.o
1304+
tpmod-objs += emc/tp/ruckig/src/ruckig/position_first_step2.o
1305+
tpmod-objs += emc/tp/ruckig/src/ruckig/position_second_step1.o
1306+
tpmod-objs += emc/tp/ruckig/src/ruckig/position_second_step2.o
1307+
tpmod-objs += emc/tp/ruckig/src/ruckig/position_third_step1.o
1308+
tpmod-objs += emc/tp/ruckig/src/ruckig/position_third_step2.o
1309+
tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_second_step1.o
1310+
tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_second_step2.o
1311+
tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_third_step1.o
1312+
tpmod-objs += emc/tp/ruckig/src/ruckig/velocity_third_step2.o
12491313
tpmod-objs += libnml/posemath/sincos.o $(MATHSTUB)
12501314

12511315
TORTOBJS = $(foreach file,$($(patsubst %.o,%,$(1))-objs), objects/rt$(file))
@@ -1266,15 +1330,40 @@ modules: $(patsubst %.o,../rtlib/%.so,$(obj-m))
12661330

12671331
RTFLAGS += -fno-strict-aliasing -fwrapv
12681332

1269-
# Rules to make .o (object) files
1270-
$(sort $(RTOBJS)) : objects/rt%.o : %.c
1333+
# Rules to make .o (object) files for C files
1334+
$(sort $(filter-out objects/rtemc/tp/ruckig_wrapper.o objects/rtemc/tp/ruckig/src/ruckig/%,$(RTOBJS))) : objects/rt%.o : %.c
12711335
$(ECHO) Compiling realtime $<
12721336
@rm -f $@
12731337
@mkdir -p $(dir $@)
12741338
$(Q)$(CC) -c $(OPT) $(DEBUG) $(EXTRA_DEBUG) -DRTAPI \
12751339
$(EXTRA_CFLAGS) \
12761340
-MP -MD -MF "${@:.o=.d}" -MT "$@" \
12771341
$< -o $@
1342+
1343+
# C++ flags for Ruckig: drop C-only -Wno-declaration-after-statement, and suppress
1344+
# false-positive -Warray-bounds and -Wunused-parameter in Ruckig library
1345+
RUCKIG_CXXFLAGS := $(filter-out -Wno-declaration-after-statement,$(EXTRA_CFLAGS)) \
1346+
-Wno-array-bounds -Wno-unused-parameter
1347+
1348+
# Rules for C++ .cc files in realtime modules
1349+
objects/rtemc/tp/ruckig_wrapper.o : emc/tp/ruckig_wrapper.cc
1350+
$(ECHO) Compiling realtime $<
1351+
@rm -f $@
1352+
@mkdir -p $(dir $@)
1353+
$(Q)$(CXX) -c $(OPT) $(DEBUG) $(EXTRA_DEBUG) -DRTAPI -std=c++20 \
1354+
$(RUCKIG_CXXFLAGS) \
1355+
-MP -MD -MF "${@:.o=.d}" -MT "$@" \
1356+
$< -o $@
1357+
1358+
# Rules for C++ .cpp files in realtime modules
1359+
objects/rtemc/tp/ruckig/src/ruckig/%.o : emc/tp/ruckig/src/ruckig/%.cpp
1360+
$(ECHO) Compiling realtime $<
1361+
@rm -f $@
1362+
@mkdir -p $(dir $@)
1363+
$(Q)$(CXX) -c $(OPT) $(DEBUG) $(EXTRA_DEBUG) -DRTAPI -std=c++20 \
1364+
$(RUCKIG_CXXFLAGS) \
1365+
-MP -MD -MF "${@:.o=.d}" -MT "$@" \
1366+
$< -o $@
12781367
endif
12791368

12801369
ifeq ($(BUILD_SYS),normal)

‎src/emc/motion-logger/Submakefile‎

Lines changed: 20 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,26 @@ MOTION_LOGGER_SRCS := \
44
$(addprefix emc/motion-logger/, motion-logger.c) \
55
emc/motion/axis.c \
66
emc/motion/simple_tp.c \
7-
emc/tp/sp_scurve.c
7+
emc/tp/sp_scurve.c \
8+
emc/tp/ruckig_wrapper.cc
89

9-
USERSRCS += $(MOTION_LOGGER_SRCS)
10+
RUCKIG_SRCS := \
11+
emc/tp/ruckig/src/ruckig/brake.cpp \
12+
emc/tp/ruckig/src/ruckig/position_first_step1.cpp \
13+
emc/tp/ruckig/src/ruckig/position_first_step2.cpp \
14+
emc/tp/ruckig/src/ruckig/position_second_step1.cpp \
15+
emc/tp/ruckig/src/ruckig/position_second_step2.cpp \
16+
emc/tp/ruckig/src/ruckig/position_third_step1.cpp \
17+
emc/tp/ruckig/src/ruckig/position_third_step2.cpp \
18+
emc/tp/ruckig/src/ruckig/velocity_second_step1.cpp \
19+
emc/tp/ruckig/src/ruckig/velocity_second_step2.cpp \
20+
emc/tp/ruckig/src/ruckig/velocity_third_step1.cpp \
21+
emc/tp/ruckig/src/ruckig/velocity_third_step2.cpp
1022

11-
../bin/motion-logger: $(call TOOBJS, $(MOTION_LOGGER_SRCS)) ../lib/libnml.so.0 ../lib/liblinuxcnchal.so.0
12-
$(ECHO) Linking $(notdir $@)
13-
$(Q)$(CC) $(LDFLAGS) -o $@ $^ -lm
23+
USERSRCS += $(MOTION_LOGGER_SRCS) $(RUCKIG_SRCS)
24+
25+
INCLUDES += emc/tp/ruckig/include
1426

27+
../bin/motion-logger: $(call TOOBJS, $(MOTION_LOGGER_SRCS) $(RUCKIG_SRCS)) ../lib/libnml.so.0 ../lib/liblinuxcnchal.so.0
28+
$(ECHO) Linking $(notdir $@)
29+
$(Q)$(CXX) $(LDFLAGS) -o $@ $^ -lm

‎src/emc/motion/motion.h‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -652,8 +652,6 @@ Suggestion: Split this in to an Error and a Status flag register..
652652
double current_acc; /* current path acceleration */
653653
double current_jerk; /* current path jerk (accurate value from TP) */
654654
double decel_dist; /* S-curve deceleration distance (dlen1) for debugging */
655-
double tc_finalvel; /* S-curve segment final velocity for debugging */
656-
double tc_maxaccel; /* S-curve segment max tangential accel for debugging */
657655
PmCartesian current_dir; /* current motion direction unit vector */
658656

659657
unsigned int tcqlen;

‎src/emc/tp/blendmath.c‎

Lines changed: 1 addition & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,7 @@
2222
#include "motion.h"
2323
#include "sp_scurve.h"
2424

25-
extern emcmot_status_t *emcmotStatus;
26-
27-
#ifndef GET_TRAJ_PLANNER_TYPE
28-
#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type)
29-
#endif
25+
#include "mot_priv.h"
3026

3127
/** @section utilityfuncs Utility functions */
3228

@@ -1214,8 +1210,6 @@ int blendComputeParameters(BlendParameters * const param)
12141210
param->d_plan = param->R_plan / tan(param->theta);
12151211

12161212
tp_debug_print("v_plan = %f\n", param->v_plan);
1217-
tp_debug_print("R_plan = %f\n", param->R_plan);
1218-
tp_debug_print("d_plan = %f\n", param->d_plan);
12191213

12201214
/* "Actual" velocity means the velocity when feed override is 1.0. Recall
12211215
* that v_plan may be greater than v_req by the max feed override. If our
@@ -1229,9 +1223,6 @@ int blendComputeParameters(BlendParameters * const param)
12291223
param->v_actual = param->v_plan;
12301224
}
12311225

1232-
// Store arc length of blend arc for future checks
1233-
param->s_arc = param->R_plan * param->phi;
1234-
12351226
if (param->R_plan < TP_POS_EPSILON) {
12361227
tp_debug_print("#Blend radius too small, aborting arc\n");
12371228
return TP_ERR_FAIL;

‎src/emc/tp/blendmath.h‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -264,15 +264,17 @@ static inline double findSCurveVPeak(double a_t_max, double j_t_max, double dist
264264
return 0.0;
265265
}
266266

267+
double triangular_v = findVPeak(a_t_max, distance);
268+
267269
double req_v;
268270
int result = findSCurveVSpeed(distance, a_t_max, j_t_max, &req_v);
269-
271+
270272
// If the S-curve calculation fails, revert to the simpler triangular velocity calculation.
271273
if (result != 1) {
272-
return findVPeak(a_t_max, distance);
274+
return triangular_v;
273275
}
274276

275277
// Take the smaller value between the S-curve velocity and the triangular velocity.
276-
return fmin(req_v, findVPeak(a_t_max, distance));
278+
return fmin(req_v, triangular_v);
277279
}
278280
#endif

‎src/emc/tp/ruckig/.github/workflows/cd.yml‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
name: CD
2+
3+
on:
4+
workflow_dispatch:
5+
release:
6+
types: [released]
7+
8+
jobs:
9+
build-wheels:
10+
# if: ${{ false }}
11+
runs-on: ${{ matrix.os }}
12+
strategy:
13+
matrix:
14+
os: [ubuntu-latest, windows-latest, macos-latest]
15+
16+
steps:
17+
- uses: actions/checkout@v6
18+
19+
- name: Build wheels
20+
uses: pypa/cibuildwheel@v3.0.1
21+
22+
- uses: actions/upload-artifact@v4
23+
with:
24+
name: wheels-${{ matrix.os }}-${{ strategy.job-index }}
25+
path: ./wheelhouse/*.whl
26+
27+
28+
make-source-dist:
29+
runs-on: ubuntu-latest
30+
31+
steps:
32+
- uses: actions/checkout@v6
33+
34+
- name: Build SDist
35+
run: pipx run build --sdist
36+
37+
- uses: actions/upload-artifact@v6
38+
with:
39+
path: dist/*.tar.gz
40+
41+
42+
upload:
43+
# if: ${{ false }}
44+
needs: [build-wheels, make-source-dist]
45+
runs-on: ubuntu-latest
46+
permissions:
47+
id-token: write
48+
49+
steps:
50+
- uses: actions/download-artifact@v6
51+
with:
52+
name: artifact
53+
path: dist
54+
55+
- name: Upload to PyPI
56+
uses: pypa/gh-action-pypi-publish@release/v1

0 commit comments

Comments
 (0)