Menu
▾
▴
[Emc-commit] upside-down: kins: Implement linear delta kinematics
|
From: Jeff E. <gi...@gi...> - 2013-08-19 20:06:09
|
kins: Implement linear delta kinematics http://git.linuxcnc.org/?p=linuxcnc.git;a=commitdiff;h=f2126b2 --- docs/man/man9/lineardeltakins.9 | 47 ++++++++++ src/Makefile | 4 + src/emc/kinematics/Submakefile | 10 ++ src/emc/kinematics/lineardeltakins-common.h | 140 ++++++++++++++++++++++++++++ src/emc/kinematics/lineardeltakins.c | 93 ++++++++++++++++++ src/emc/kinematics/lineardeltakins.cc | 54 +++++++++++ 6 files changed, 348 insertions(+) diff --git a/docs/man/man9/lineardeltakins.9 b/docs/man/man9/lineardeltakins.9 new file mode 100644 index 0000000..50d8af4 --- /dev/null +++ b/docs/man/man9/lineardeltakins.9 @@ -0,0 +1,47 @@ +.TH LINEARDELTAKINS "9" "2013-07-11" "LinuxCNC Documentation" "Kinematics Component" +.de TQ +.br +.ns +.TP \\$1 +.. + +.SH NAME + +lineardeltakins \- Kinematics for a linear delta robot + +.SH SYNOPSIS +.HP +.B loadrt lineardeltakins + +.SH KINEMATICS +The kinematics model is appropriate for a rostock/kossel-style design +with three joints arranged in an equilateral triangle. (0,0) is always +the center of the working volume. Joint 0 is at (0,R) and subsequent +joints are 120 degrees clockwise (note that joint 0 is not at zero +radians). The length of the arm is L. + +Joints 0-2 are the linear carriages. Axes ABC and UVW are passed +through unchanged in joints 3-8, so that e.g., A can still be used to +control an extruder. + +.SH PINS +.TP +.B lineardeltakins.R\fR float in +Effective diameter of the platform. + +R is different than the distance from the center of the table to the +center of the belt/smooth rod/extrusion that the joints ride on. In +RepRap delta parlance, R is DELTA_RADIUS which is computed as +DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET. + +.TP +.B lineardeltakins.L\fR float in +Length of the rod connecting the carriage to the effector. In RepRap +delta parlance, L is DELTA_DIAGONAL_ROD + +.SH NOTES +The R and L values can be adjusted while LinuxCNC is running. However, +doing so while in coordinated mode will lead to a step change in joint +position, which generally will trigger a following error if in joint +mode with machine on. + diff --git a/src/Makefile b/src/Makefile index 7f0e3b3..9c45512 100644 --- a/src/Makefile +++ b/src/Makefile @@ -845,6 +845,9 @@ rotatekins-objs := emc/kinematics/rotatekins.o obj-m += tripodkins.o tripodkins-objs := emc/kinematics/tripodkins.o +obj-m += lineardeltakins.o +lineardeltakins-objs := emc/kinematics/lineardeltakins.o + obj-m += genhexkins.o genhexkins-objs := emc/kinematics/genhexkins.o genhexkins-objs += libnml/posemath/_posemath.o @@ -981,6 +984,7 @@ endif ../rtlib/gantrykins$(MODULE_EXT): $(addprefix objects/rt,$(gantrykins-objs)) ../rtlib/rotatekins$(MODULE_EXT): $(addprefix objects/rt,$(rotatekins-objs)) ../rtlib/tripodkins$(MODULE_EXT): $(addprefix objects/rt,$(tripodkins-objs)) +../rtlib/lineardeltakins$(MODULE_EXT): $(addprefix objects/rt,$(lineardeltakins-objs)) ../rtlib/genhexkins$(MODULE_EXT): $(addprefix objects/rt,$(genhexkins-objs)) ../rtlib/genserkins$(MODULE_EXT): $(addprefix objects/rt,$(genserkins-objs)) ../rtlib/pumakins$(MODULE_EXT): $(addprefix objects/rt,$(pumakins-objs)) diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 68a9858..9f63058 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -4,6 +4,16 @@ GENSERKINSSRCS := \ emc/kinematics/genserkins.c USERSRCS += $(GENSERKINSSRCS) +DELTAMODULESRCS := emc/kinematics/lineardeltakins.cc +PYSRCS += $(DELTAMODULESRCS) +$(call TOOBJS, $(DELTAMODULESRCS)): CFLAGS += -x c++ -Wno-declaration-after-statement + +DELTAMODULE := ../lib/python/lineardeltakins.so +$(DELTAMODULE): $(call TOOBJS, $(DELTAMODULESRCS)) + $(ECHO) Linking python module $(notdir $@) + $(CXX) $(LDFLAGS) -shared -o $@ $^ $(BOOST_PYTHON_LIBS) +PYTARGETS += $(DELTAMODULE) + ../bin/genserkins: $(call TOOBJS, $(GENSERKINSSRCS)) ../lib/liblinuxcnchal.so ../lib/libposemath.so $(ECHO) Linking $(notdir $@) $(Q)$(CC) $(LDFLAGS) -o $@ $^ diff --git a/src/emc/kinematics/lineardeltakins-common.h b/src/emc/kinematics/lineardeltakins-common.h new file mode 100644 index 0000000..0ab4fce --- /dev/null +++ b/src/emc/kinematics/lineardeltakins-common.h @@ -0,0 +1,140 @@ +#ifndef LINUXCNCLINEARDELTAKINS_COMMON_H +#define LINUXCNCLINEARDELTAKINS_COMMON_H +// Copyright 2013 Jeff Epler <je...@un...> +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +/* + * Kinematics for a rostock-style delta robot + * + * Towers 0, 1 and 2 are spaced at 120 degrees around the origin + * at distance R. A rod of length L (L > R) connects each tower to the + * platform. Tower 0 is at (0,R). (note: this is not at zero radians!) + * + * ABCUVW coordinates are passed through in joints[3..8]. + * + * L is like DELTA_DIAGONAL_ROD and R is like DELTA_RADIUS in + * Marlin---remember to account for the effector and carriage offsets + * when changing from the default. + */ + +// common routines used by the userspace kinematics and the realtime kinematics +// user must include a math.h-type header first +// Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 +#include "emcpos.h" + +static double L, R; +static double Ax, Ay, Bx, By, Cx, Cy, L2; + +#define SQ3 (sqrt(3)) + +#define SIN_60 (SQ3/2) +#define COS_60 (.5) + +static double sq(double x) { return x*x; } + +static void set_geometry(double r_, double l_) +{ + if(L == l_ && R == r_) return; + + L = l_; + R = r_; + + L2 = sq(L); + + Ax = 0.0; + Ay = R; + + Bx = -SIN_60 * R; + By = -COS_60 * R; + + Cx = SIN_60 * R; + Cy = -COS_60 * R; +} + +static int kinematics_inverse(const EmcPose *pos, double *joints) +{ + double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; + joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); + joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); + joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); + joints[3] = pos->a; + joints[4] = pos->b; + joints[5] = pos->c; + joints[6] = pos->u; + joints[7] = pos->v; + joints[8] = pos->w; + + return isnan(joints[0]) || isnan(joints[1]) || isnan(joints[2]) + ? -1 : 0; +} + +static int kinematics_forward(const double *joints, EmcPose *pos) +{ + double q1 = joints[0]; + double q2 = joints[1]; + double q3 = joints[2]; + + double den = (By-Ay)*Cx-(Cy-Ay)*Bx; + + double w1 = Ay*Ay + q1*q1; // n.b. assumption that Ax is 0 all through here + double w2 = Bx*Bx + By*By + q2*q2; + double w3 = Cx*Cx + Cy*Cy + q3*q3; + + double a1 = (q2-q1)*(Cy-Ay)-(q3-q1)*(By-Ay); + double b1 = -((w2-w1)*(Cy-Ay)-(w3-w1)*(By-Ay))/2.0; + + double a2 = -(q2-q1)*Cx+(q3-q1)*Bx; + double b2 = ((w2-w1)*Cx - (w3-w1)*Bx)/2.0; + + // a*z^2 + b*z + c = 0 + double a = a1*a1 + a2*a2 + den*den; + double b = 2*(a1*b1 + a2*(b2-Ay*den) - q1*den*den); + double c = (b2-Ay*den)*(b2-Ay*den) + b1*b1 + den*den*(q1*q1 - L*L); + + double discr = b*b - 4.0*a*c; + if (discr < 0) return -1; // non-existing point + + double z = -0.5*(b+sqrt(discr))/a; + pos->tran.z = z; + pos->tran.x = (a1*z + b1)/den; + pos->tran.y = (a2*z + b2)/den; + pos->a = joints[3]; + pos->b = joints[4]; + pos->c = joints[5]; + pos->u = joints[6]; + pos->v = joints[7]; + pos->w = joints[8]; + + return 0; +} + +// Default values which may correspond to someone's linear delta robot. To +// change these, use halcmd setp rather than rebuilding the software. + +// Center-to-center distance of the holes in the diagonal push rods. +#define DELTA_DIAGONAL_ROD 269.0 // mm + +// Horizontal offset from middle of printer to smooth rod center. +#define DELTA_SMOOTH_ROD_OFFSET 198.25 // mm + +// Horizontal offset of the universal joints on the end effector. +#define DELTA_EFFECTOR_OFFSET 33.0 // mm + +// Horizontal offset of the universal joints on the carriages. +#define DELTA_CARRIAGE_OFFSET 35.0 // mm + +// Effective horizontal distance bridged by diagonal push rods. +#define DELTA_RADIUS (DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET) +#endif diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c new file mode 100644 index 0000000..df36ae4 --- /dev/null +++ b/src/emc/kinematics/lineardeltakins.c @@ -0,0 +1,93 @@ +// Copyright 2013 Jeff Epler <je...@un...> +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +#include "hal.h" +#include "kinematics.h" +#include "rtapi_math.h" +#include "rtapi_app.h" + +#include "lineardeltakins-common.h" + +struct haldata +{ + hal_float_t *r, *l; +} *haldata; + +int comp_id; + +int kinematicsForward(const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + set_geometry(*haldata->r, *haldata->l); + return kinematics_forward(joints, pos); +} + +int kinematicsInverse(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + set_geometry(*haldata->r, *haldata->l); + return kinematics_inverse(pos, joints); +} + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_BOTH; +} + +int rtapi_app_main(void) +{ + int retval = 0; + + comp_id = hal_init("lineardeltakins"); + if(comp_id < 0) retval = comp_id; + + if(retval == 0) + { + haldata = hal_malloc(sizeof(struct haldata)); + retval = !haldata; + } + + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->r, comp_id, + "lineardeltakins.R"); + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->l, comp_id, + "lineardeltakins.L"); + + if(retval == 0) + { + *haldata->r = DELTA_RADIUS; + *haldata->l = DELTA_DIAGONAL_ROD; + } + + if(retval == 0) + { + hal_ready(comp_id); + } + + return retval; +} + +void rtapi_app_exit(void) +{ + hal_exit(comp_id); +} + +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/lineardeltakins.cc b/src/emc/kinematics/lineardeltakins.cc new file mode 100644 index 0000000..325378f --- /dev/null +++ b/src/emc/kinematics/lineardeltakins.cc @@ -0,0 +1,54 @@ +// Copyright 2013 Jeff Epler <je...@un...> +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +#include <cmath> +#include "lineardeltakins-common.h" +#include <boost/python.hpp> +using namespace boost::python; + +static object forward(double j0, double j1, double j2) +{ + double joints[9] = {j0, j1, j2}; + EmcPose pos; + int result = kinematics_forward(joints, &pos); + if(result == 0) + return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); + return object(); +} + +static object inverse(double x, double y, double z) +{ + double joints[9]; + EmcPose pos = {{x,y,z}}; + int result = kinematics_inverse(&pos, joints); + if(result == 0) + return make_tuple(joints[0], joints[1], joints[2]); + return object(); +} + +static object get_geometry() +{ + return make_tuple(R, L); +} + +BOOST_PYTHON_MODULE(lineardeltakins) +{ + set_geometry(DELTA_RADIUS, DELTA_DIAGONAL_ROD); + def("set_geometry", set_geometry); + def("get_geometry", get_geometry); + def("forward", forward); + def("inverse", inverse); +} |