/***************************************************************** * Description: scarakins.c * Kinematics for scara typed robots * Set the params using HAL to fit your robot * * Derived from a work by Sagar Behere * * Author: Sagar Behere * License: GPL Version 2 * System: Linux * * Copyright (c) 2003 All rights reserved. * * Last change: ******************************************************************* */ #include "rtapi.h" #include "rtapi_math.h" #include "rtapi_string.h" #include "posemath.h" #include "hal.h" #include "kinematics.h" #include "switchkins.h" struct scara_data { hal_float_t *d1, *d2, *d3, *d4, *d5, *d6; } *haldata = 0; /* key dimensions joint[0] = Entire arm rotates around a vertical axis at its inner end which is attached to the earth. A value of zero means the inner arm is pointing along the X axis. D1 = Vertical distance from the ground plane to the center of the inner arm. D2 = Horizontal distance between joint[0] axis and joint[1] axis, ie. the length of the inner arm. joint[1] = Outer arm rotates around a vertical axis at its inner end which is attached to the outer end of the inner arm. A value of zero means the outer arm is parallel to the inner arm (and extending outward). D3 = Vertical distance from the center of the inner arm to the center of the outer arm. May be positive or negative depending on the structure of the robot. joint[2] = End effector slides along a vertical axis at the outer end of the outer arm. A value of zero means the end effector is at the same height as the center of the outer arm, and positive values mean downward movement. D4 = Horizontal distance between joint[1] axis and joint[2] axis, ie. the length of the outer arm joint[3] = End effector rotates around the same vertical axis that it slides along. A value of zero means that the tooltip (if offset from the axis) is pointing in the same direction as the centerline of the outer arm. D5 = Vertical distance from the end effector to the tooltip. Positive means the tooltip is lower than the end effector, and is the normal case. D6 = Horizontal distance from the centerline of the end effector (and the joints 2 and 3 axis) and the tooltip. Zero means the tooltip is on the centerline. Non-zero values should be positive, if negative they introduce a 180 degree offset on the value of joint[3]. */ #define D1 (*(haldata->d1)) #define D2 (*(haldata->d2)) #define D3 (*(haldata->d3)) #define D4 (*(haldata->d4)) #define D5 (*(haldata->d5)) #define D6 (*(haldata->d6)) /* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ static int scaraKinematicsForward(const double * joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { double a0, a1, a3; double x, y, z, c; /* convert joint angles to radians for sin() and cos() */ a0 = joint[0] * ( PM_PI / 180 ); a1 = joint[1] * ( PM_PI / 180 ); a3 = joint[3] * ( PM_PI / 180 ); /* convert angles into world coords */ a1 = a1 + a0; a3 = a3 + a1; x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); z = D1 + D3 - joint[2] - D5; c = a3; *iflags = 0; if (joint[1] < 90) *iflags = 1; world->tran.x = x; world->tran.y = y; world->tran.z = z; world->c = c * 180 / PM_PI; world->a = joint[4]; world->b = joint[5]; return (0); } //scaraKinematicsForward() static int scaraKinematicsInverse(const EmcPose * world, double * joint, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { double a3; double q0, q1; double xt, yt, rsq, cc; double x, y, z, c; x = world->tran.x; y = world->tran.y; z = world->tran.z; c = world->c; /* convert degrees to radians */ a3 = c * ( PM_PI / 180 ); /* center of end effector (correct for D6) */ xt = x - D6*cos(a3); yt = y - D6*sin(a3); /* horizontal distance (squared) from end effector centerline to main column centerline */ rsq = xt*xt + yt*yt; /* joint 1 angle needed to make arm length match sqrt(rsq) */ cc = (rsq - D2*D2 - D4*D4) / (2*D2*D4); if(cc < -1) cc = -1; if(cc > 1) cc = 1; q1 = acos(cc); if (*iflags) q1 = -q1; /* angle to end effector */ q0 = atan2(yt, xt); /* end effector coords in inner arm coord system */ xt = D2 + D4*cos(q1); yt = D4*sin(q1); /* inner arm angle */ q0 = q0 - atan2(yt, xt); /* q0 and q1 are still in radians. convert them to degrees */ q0 = q0 * (180 / PM_PI); q1 = q1 * (180 / PM_PI); joint[0] = q0; joint[1] = q1; joint[2] = D1 + D3 - D5 - z; joint[3] = c - ( q0 + q1); joint[4] = world->a; joint[5] = world->b; *fflags = 0; return (0); } // scaraKinematicsInverse() #define DEFAULT_D1 490 #define DEFAULT_D2 340 #define DEFAULT_D3 50 #define DEFAULT_D4 250 #define DEFAULT_D5 50 #define DEFAULT_D6 50 static int scaraKinematicsSetup(const int comp_id, const char* coordinates, kparms* kp) { int res=0; haldata = hal_malloc(sizeof(*haldata)); if (!haldata) goto error; res += hal_pin_float_newf(HAL_IN, &(haldata->d1), comp_id,"%s.D1",kp->halprefix); res += hal_pin_float_newf(HAL_IN, &(haldata->d2), comp_id,"%s.D2",kp->halprefix); res += hal_pin_float_newf(HAL_IN, &(haldata->d3), comp_id,"%s.D3",kp->halprefix); res += hal_pin_float_newf(HAL_IN, &(haldata->d4), comp_id,"%s.D4",kp->halprefix); res += hal_pin_float_newf(HAL_IN, &(haldata->d5), comp_id,"%s.D5",kp->halprefix); res += hal_pin_float_newf(HAL_IN, &(haldata->d6), comp_id,"%s.D6",kp->halprefix); if (res) { goto error; } D1 = DEFAULT_D1; D2 = DEFAULT_D2; D3 = DEFAULT_D3; D4 = DEFAULT_D4; D5 = DEFAULT_D5; D6 = DEFAULT_D6; return 0; error: return -1; } // scaraKinematicsSetup() int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, KI* kinv0, KI* kinv1, KI* kinv2 ) { kp->kinsname = "scarakins"; // !!! must agree with filename kp->halprefix = "scarakins"; // hal pin names kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = scaraKinematicsSetup; *kfwd0 = scaraKinematicsForward; *kinv0 = scaraKinematicsInverse; *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; *kinv1 = identityKinematicsInverse; *kset2 = userkKinematicsSetup; *kfwd2 = userkKinematicsForward; *kinv2 = userkKinematicsInverse; return 0; } // switchkinsSetup()