#include "posemath.h"#include "emcpos.h"#include "cubic.h"#include "pid.h"#include "emcmotcfg.h"#include "emcmotlog.h"#include "tp.h"#include "tc.h"#include "mmxavg.h"Include dependency graph for emcmot.h:
This graph shows which files directly or indirectly include this file:
Go to the source code of this file.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Definition at line 107 of file emcmot.h. 00107 {
00108 EMCMOT_SET_TRAJ_CYCLE_TIME = 1, /* set the cycle time */
00109 EMCMOT_SET_SERVO_CYCLE_TIME, /* set the interpolation rate */
00110 EMCMOT_SET_POSITION_LIMITS, /* set the axis position +/- limits */
00111 EMCMOT_SET_OUTPUT_LIMITS, /* set the axis output +/- limits */
00112 EMCMOT_SET_OUTPUT_SCALE, /* scale factor for outputs */
00113 EMCMOT_SET_INPUT_SCALE, /* scale factor for inputs */
00114 EMCMOT_SET_MIN_FERROR, /* minimum following error, input units */
00115 EMCMOT_SET_MAX_FERROR, /* maximum following error, input units */
00116 EMCMOT_JOG_CONT, /* continuous jog */
00117 EMCMOT_JOG_INCR, /* incremental jog */
00118 EMCMOT_JOG_ABS, /* absolute jog */
00119 EMCMOT_SET_LINE, /* queue up a linear move */
00120 EMCMOT_SET_CIRCLE, /* queue up a circular move */
00121 EMCMOT_SET_VEL, /* set the velocity for subsequent moves */
00122 EMCMOT_SET_VEL_LIMIT, /* set the absolute max vel for all moves */
00123 EMCMOT_SET_AXIS_VEL_LIMIT, /* set the absolute max vel for each axis */
00124 EMCMOT_SET_ACC, /* set the acceleration for moves */
00125 EMCMOT_PAUSE, /* pause motion */
00126 EMCMOT_RESUME, /* resume motion */
00127 EMCMOT_STEP, /* resume motion until id encountered */
00128 EMCMOT_ABORT, /* abort motion */
00129 EMCMOT_SCALE, /* scale the speed */
00130 EMCMOT_ENABLE, /* enable servos for active axes */
00131 EMCMOT_DISABLE, /* disable servos for active axes */
00132 EMCMOT_SET_PID, /* set PID gains */
00133 EMCMOT_ENABLE_AMPLIFIER, /* enable amp outputs and dac writes */
00134 EMCMOT_DISABLE_AMPLIFIER, /* disable amp outputs and dac writes */
00135 EMCMOT_OPEN_LOG, /* open a log */
00136 EMCMOT_START_LOG, /* start logging */
00137 EMCMOT_STOP_LOG, /* stop logging */
00138 EMCMOT_CLOSE_LOG, /* close log */
00139 EMCMOT_DAC_OUT, /* write directly to the dacs */
00140 EMCMOT_HOME, /* home an axis */
00141 EMCMOT_FREE, /* set mode to free (joint) motion */
00142 EMCMOT_COORD, /* set mode to coordinated motion */
00143 EMCMOT_TELEOP, /* set mode to teleop*/
00144 EMCMOT_ENABLE_WATCHDOG, /* enable watchdog sound, parport */
00145 EMCMOT_DISABLE_WATCHDOG, /* enable watchdog sound, parport */
00146 EMCMOT_SET_POLARITY, /* set polarity for axis flags */
00147 EMCMOT_ACTIVATE_AXIS, /* make axis active */
00148 EMCMOT_DEACTIVATE_AXIS, /* make axis inactive */
00149 EMCMOT_SET_TERM_COND, /* set termination condition (stop, blend) */
00150 EMCMOT_SET_HOMING_VEL, /* set the axis homing speed */
00151 EMCMOT_SET_NUM_AXES, /* set the number of axes */
00152 EMCMOT_SET_WORLD_HOME, /* set pose for world home */
00153 EMCMOT_SET_JOINT_HOME, /* set value for joint homes */
00154 EMCMOT_SET_HOME_OFFSET, /* where to go after a home */
00155 EMCMOT_OVERRIDE_LIMITS, /* temporarily ignore limits until jog done */
00156 EMCMOT_SET_TELEOP_VECTOR, /* Move at a given velocity but in
00157 world cartesian coordinates, not in joint
00158 space like EMCMOT_JOG_* */
00159 #ifdef ENABLE_PROBING
00160 EMCMOT_SET_PROBE_INDEX, /* set which wire the probe signal is on. */
00161 EMCMOT_SET_PROBE_POLARITY, /* probe tripped on 0 to 1 transition or on
00162 1 to 0 transition. */
00163 EMCMOT_CLEAR_PROBE_FLAGS, /* clears probeTripped flag */
00164 EMCMOT_PROBE, /* go towards a position, stop if the probe
00165 is tripped, and record the position where
00166 the probe tripped */
00167 EMCMOT_SET_DEBUG, /* sets the debug level */
00168 EMCMOT_SET_AOUT, /* sets an analog motion point for next move */
00169 EMCMOT_SET_DOUT /* sets a digital motion point for next move */
00170 #endif
00171
00172 };
|
|
|
Definition at line 348 of file emcmot.h. 00348 {
00349 KINEMATICS_IDENTITY = 1, /* forward=inverse, both well-behaved */
00350 KINEMATICS_FORWARD_ONLY, /* forward but no inverse */
00351 KINEMATICS_INVERSE_ONLY, /* inverse but no forward */
00352 KINEMATICS_BOTH /* forward and inverse both */
00353 } KINEMATICS_TYPE;
|
|
|
Definition at line 2904 of file emcsegmot.c. 02905 {
02906 int axis;
02907
02908 /* release traj planner space */
02909 #if !defined(rtlinux)
02910 free(sqMemPool);
02911 #endif
02912 sqTrashQueue(&queue);
02913
02914 /* release axis planner spaces */
02915 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
02916 {
02917 tpDelete(&freeAxis[axis]);
02918 }
02919
02920 /* disable amps */
02921 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
02922 {
02923 extAmpEnable(axis,
02924 ! GET_AXIS_ENABLE_POLARITY(axis));
02925 }
02926
02927 /* quit board */
02928 extAioQuit();
02929 extDioQuit();
02930 extMotQuit();
02931
02932 #ifdef rtlinux
02933
02934 #ifdef RT_FIFO
02935 rtf_destroy(EMCMOT_COMMAND_RTF);
02936 rtf_destroy(EMCMOT_STATUS_RTF);
02937 rtf_destroy(EMCMOT_ERROR_RTF);
02938 #endif
02939
02940 /* delete control task */
02941 rt_task_delete(&emcmotTask);
02942
02943 /* FIXME */
02944 diagnostics("exited emcmot\n");
02945
02946 #ifdef STEPPER_MOTORS
02947
02948 /* delete stepper motor task */
02949 rt_task_delete(&smTask);
02950
02951 #endif
02952
02953 #else
02954
02955 /* detach from shmem */
02956 if (NULL != shmem)
02957 {
02958 rcs_shm_close(shmem);
02959 shmem = NULL;
02960 }
02961
02962 #endif
02963 }
|
|
||||||||||||
|
Definition at line 77 of file emcmotutil.c. 00078 {
00079 char *p1;
00080 const char *p2;
00081 int i;
00082 if (errlog == 0 ||
00083 errlog->num == 0) {
00084 /* empty */
00085 return -1;
00086 }
00087
00088 errlog->head++;
00089
00090 // strncpy(error, errlog->error[errlog->start], EMCMOT_ERROR_LEN);
00091 // replaced strncpy with manual copy
00092 p1=error;
00093 p2=errlog->error[errlog->start];
00094 i=0;
00095 while(*p2 && i < EMCMOT_ERROR_LEN)
00096 {
00097 *p1 = *p2;
00098 p1++;
00099 p2++;
00100 i++;
00101 }
00102 *p1=0;
00103
00104
00105 errlog->start = (errlog->start + 1) % EMCMOT_ERROR_NUM;
00106 errlog->num--;
00107
00108 errlog->tail = errlog->head;
00109
00110 return 0;
00111 }
|
|
|
Referenced by init_module().
|
|
||||||||||||
|
Definition at line 41 of file emcmotutil.c. 00042 {
00043 char *p1;
00044 const char *p2;
00045 int i;
00046
00047 if (errlog == 0 ||
00048 errlog->num == EMCMOT_ERROR_NUM) {
00049 /* full */
00050 return -1;
00051 }
00052
00053 errlog->head++;
00054
00055 // strncpy(errlog->error[errlog->end], error, EMCMOT_ERROR_LEN);
00056 // replaced strncpy with manual copy
00057 p1=errlog->error[errlog->end];
00058 p2=error;
00059 i=0;
00060 while(*p2 && i < EMCMOT_ERROR_LEN)
00061 {
00062 *p1 = *p2;
00063 p1++;
00064 p2++;
00065 i++;
00066 }
00067 *p1=0;
00068
00069 errlog->end = (errlog->end + 1) % EMCMOT_ERROR_NUM;
00070 errlog->num++;
00071
00072 errlog->tail = errlog->head;
00073
00074 return 0;
00075 }
|
|
|
Definition at line 2570 of file emcsegmot.c. 02571 {
02572 int axis;
02573 int t;
02574 #ifdef rtlinux
02575 RTIME now;
02576 #endif
02577 PID_STRUCT pid;
02578
02579 #ifdef rtlinux
02580
02581 #ifdef RT_FIFO
02582 rtf_create(EMCMOT_COMMAND_RTF, sizeof(EMCMOT_COMMAND) + 1);
02583 rtf_create(EMCMOT_STATUS_RTF, sizeof(EMCMOT_STATUS) + 1);
02584 rtf_create(EMCMOT_ERROR_RTF, EMCMOT_ERROR_NUM * EMCMOT_ERROR_LEN + 1);
02585 /* log is created, deleted dynamically */
02586
02587 /* fifo puts and gets work on buffer structs, so point to these */
02588 emcmotCommand = &emcmotCommandBuffer;
02589 emcmotStatus = &emcmotStatusBuffer;
02590
02591 #else /* not RT_FIFO */
02592 /* set upper memory pointers */
02593 emcmotStruct = (EMCMOT_STRUCT *) SHMEM_BASE_ADDRESS;
02594
02595 #endif /* else not RT_FIFO */
02596
02597 #else /* not rtlinux */
02598 /* get command shmem */
02599 shmem = rcs_shm_open(SHMEM_KEY, sizeof(EMCMOT_STRUCT), 1, 0666);
02600 if (NULL == shmem ||
02601 NULL == shmem->addr)
02602 {
02603 diagnostics("can't get shared memory\n");
02604 return -1;
02605 }
02606
02607 memset(shmem->addr, 0, sizeof(EMCMOT_STRUCT));
02608 /* map shmem area into local address space */
02609 emcmotStruct = (EMCMOT_STRUCT *) shmem->addr;
02610
02611 #endif /* else not rtlinux */
02612
02613 #ifndef RT_FIFO
02614 /* we'll reference emcmotStruct directly */
02615 emcmotCommand = (EMCMOT_COMMAND *) &emcmotStruct->command;
02616 emcmotStatus = (EMCMOT_STATUS *) &emcmotStruct->status;
02617 emcmotError = (EMCMOT_ERROR *) &emcmotStruct->error;
02618 emcmotLog = (EMCMOT_LOG *) &emcmotStruct->log;
02619
02620 /* zero shared memory */
02621 for (t = 0; t < sizeof(EMCMOT_STRUCT); t++)
02622 {
02623 ((char *) emcmotStruct)[t] = 0;
02624 }
02625 #endif /* not RT_FIFO */
02626
02627 /* init locals */
02628 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
02629 {
02630 maxLimitSwitchCount[axis] = 0;
02631 minLimitSwitchCount[axis] = 0;
02632 /* STEPPER MOTORS */
02633 smStepsPerUnit[axis] = 1.0;
02634 smStepsAccum[axis] = 0;
02635 smClkPhase[axis] = 0;
02636 /* END STEPPER MOTORS */
02637 }
02638
02639 #ifndef RT_FIFO
02640 /* init error struct */
02641 emcmotErrorInit(emcmotError);
02642 #endif /* not RT_FIFO */
02643
02644 /* init command struct */
02645 emcmotCommand->head = 0;
02646 emcmotCommand->command = 0;
02647 emcmotCommand->commandNum = 0;
02648 emcmotCommand->tail = 0;
02649
02650 /* init status struct */
02651
02652 emcmotStatus->head = 0;
02653
02654 emcmotStatus->motionFlag = 0;
02655 SET_MOTION_ERROR_FLAG(0);
02656 SET_MOTION_COORD_FLAG(0);
02657 emcmotStatus->split = 0;
02658 emcmotStatus->commandEcho = 0;
02659 emcmotStatus->commandNumEcho = 0;
02660 emcmotStatus->commandStatus = 0;
02661 emcmotStatus->heartbeat = 0;
02662 emcmotStatus->computeTime = 0.0;
02663 emcmotStatus->numAxes = EMCMOT_MAX_AXIS;
02664 emcmotStatus->trajCycleTime = TRAJ_CYCLE_TIME;
02665 emcmotStatus->servoCycleTime = SERVO_CYCLE_TIME;
02666 emcmotStatus->pos.tran.x = 0.0;
02667 emcmotStatus->pos.tran.y = 0.0;
02668 emcmotStatus->pos.tran.z = 0.0;
02669 emcmotStatus->actualPos.tran.x = 0.0;
02670 emcmotStatus->actualPos.tran.y = 0.0;
02671 emcmotStatus->actualPos.tran.z = 0.0;
02672 emcmotStatus->vel = VELOCITY;
02673 emcmotStatus->limitVel = VELOCITY;
02674 emcmotStatus->acc = ACCELERATION;
02675 emcmotStatus->qVscale = 1.0;
02676 emcmotStatus->id = 0;
02677 emcmotStatus->depth = 0;
02678 emcmotStatus->activeDepth = 0;
02679 emcmotStatus->paused = 0;
02680 SET_MOTION_INPOS_FLAG(1);
02681 emcmotStatus->logOpen = 0;
02682 emcmotStatus->logStarted = 0;
02683 emcmotStatus->logSize = 0;
02684 emcmotStatus->logSkip = 0;
02685 emcmotStatus->logPoints = 0;
02686 SET_MOTION_ENABLE_FLAG(0);
02687
02688 oldPos = emcmotStatus->pos;
02689 oldVel.tran.x = 0.0;
02690 oldVel.tran.y = 0.0;
02691 oldVel.tran.z = 0.0;
02692
02693 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
02694 {
02695 emcmotStatus->homingVel[axis] = VELOCITY;
02696 emcmotStatus->axisFlag[axis] = 0;
02697 emcmotStatus->maxLimit[axis] = MAX_LIMIT;
02698 emcmotStatus->minLimit[axis] = MIN_LIMIT;
02699 emcmotStatus->maxOutput[axis] = MAX_OUTPUT;
02700 emcmotStatus->minOutput[axis] = MIN_OUTPUT;
02701 emcmotStatus->maxFerror[axis] = MAX_FERROR;
02702 emcmotStatus->ferror[axis] = 0.0;
02703 emcmotStatus->outputScale[axis] = OUTPUT_SCALE;
02704 emcmotStatus->outputOffset[axis] = OUTPUT_OFFSET;
02705 emcmotStatus->inputScale[axis] = INPUT_SCALE;
02706 emcmotStatus->inputOffset[axis] = INPUT_OFFSET;
02707 emcmotStatus->axVscale[axis] = 1.0;
02708 SET_AXIS_ENABLE_FLAG(axis, 0);
02709 SET_AXIS_ACTIVE_FLAG(axis, 1); /* default is use it */
02710 SET_AXIS_NSL_FLAG(axis, 0);
02711 SET_AXIS_PSL_FLAG(axis, 0);
02712 SET_AXIS_NHL_FLAG(axis, 0);
02713 SET_AXIS_PHL_FLAG(axis, 0);
02714 SET_AXIS_INPOS_FLAG(axis, 1);
02715 SET_AXIS_HOMING_FLAG(axis, 0);
02716 SET_AXIS_HOMED_FLAG(axis, 0);
02717 SET_AXIS_FERROR_FLAG(axis, 0);
02718 SET_AXIS_FAULT_FLAG(axis, 0);
02719 SET_AXIS_ERROR_FLAG(axis, 0);
02720 emcmotStatus->axisPolarity[axis] = (EMCMOT_AXIS_FLAG) 0xFFFFFFFF;
02721 /* will read encoders directly, so don't set them here */
02722 }
02723
02724 /* FIXME-- add emcmotError */
02725
02726 /* init min-max-avg stats */
02727 mmxavgInit(&tMmxavg, tMmxavgSpace, MMXAVG_SIZE);
02728 mmxavgInit(&sMmxavg, sMmxavgSpace, MMXAVG_SIZE);
02729 mmxavgInit(&nMmxavg, nMmxavgSpace, MMXAVG_SIZE);
02730 emcmotStatus->tMin = 0.0;
02731 emcmotStatus->tMax = 0.0;
02732 emcmotStatus->tAvg = 0.0;
02733 emcmotStatus->sMin = 0.0;
02734 emcmotStatus->sMax = 0.0;
02735 emcmotStatus->sAvg = 0.0;
02736 emcmotStatus->nMin = 0.0;
02737 emcmotStatus->nMax = 0.0;
02738 emcmotStatus->nAvg = 0.0;
02739
02740 /* init motion queue */
02741 #ifdef rtlinux
02742 if ( -1 == sqInitQueue(&queue,(SEGMENT *)(SHMEM_BASE_ADDRESS+sizeof(EMCMOT_STRUCT)), TC_QUEUE_SIZE))
02743 #else
02744 sqMemPool = (SEGMENT *)malloc(TC_QUEUE_SIZE*sizeof(SEGMENT));
02745 if ( sqMemPool == NULL )
02746 {
02747 diagnostics("Error in init_module(): cannot allocate memory for segmentqueue\n");
02748 return -1;
02749 }
02750 if ( -1 == sqInitQueue(&queue,sqMemPool, TC_QUEUE_SIZE))
02751 #endif
02752 {
02753 diagnostics("can't initialize motion queue\n");
02754 return -1;
02755 }
02756 sqSetCycleTime(&queue, emcmotStatus->trajCycleTime);
02757 sqSetPos(&queue, emcmotStatus->pos);
02758 sqSetVmax(&queue, emcmotStatus->vel);
02759 sqSetFeed(&queue, emcmotStatus->vel);
02760 /* FIX ME ^^^^ the feed and the absolute maximum velocity
02761 are set the same number */
02762
02763 sqSetMaxAcc(&queue, emcmotStatus->acc);
02764 sqSetMaxFeedOverride(&queue, 1.0 );
02765
02766 /* init the axis components */
02767 pid.p = P_GAIN;
02768 pid.i = I_GAIN;
02769 pid.d = D_GAIN;
02770 pid.ff0 = FF0_GAIN;
02771 pid.ff1 = FF1_GAIN;
02772 pid.ff2 = FF2_GAIN;
02773 pid.backlash = BACKLASH;
02774 pid.bias = BIAS;
02775 pid.maxError = MAX_ERROR;
02776
02777 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
02778 {
02779 if (-1 == tpCreate(&freeAxis[axis], FREE_AXIS_QUEUE_SIZE, freeAxisTcSpace[axis]))
02780 {
02781 diagnostics("can't create axis queue %d\n", axis);
02782 return -1;
02783 }
02784 tpInit(&freeAxis[axis]);
02785 tpSetCycleTime(&freeAxis[axis], emcmotStatus->trajCycleTime);
02786 tpSetPos(&freeAxis[axis], emcmotStatus->pos);
02787 tpSetVmax(&freeAxis[axis], emcmotStatus->vel);
02788 tpSetAmax(&freeAxis[axis], emcmotStatus->acc);
02789 pidInit(&emcmotStatus->pid[axis]);
02790 pidSetGains(&emcmotStatus->pid[axis], pid);
02791 cubicInit(&cubic[axis]);
02792 }
02793
02794 /* init the time and rate using functions to affect traj, the
02795 pids, and the cubics properly, since they're coupled */
02796 setTrajCycleTime(TRAJ_CYCLE_TIME);
02797 setServoCycleTime(SERVO_CYCLE_TIME);
02798
02799 /* init board */
02800 if (-1 == extMotInit((const char *) 0))
02801 {
02802 diagnostics("can't initialize motion hardware\n");
02803 return -1;
02804 }
02805 if (-1 == extDioInit((const char *) 0))
02806 {
02807 diagnostics("can't initialize DIO hardware\n");
02808 return -1;
02809 }
02810 if (-1 == extAioInit((const char *) 0))
02811 {
02812 diagnostics("can't initialize AIO hardware\n");
02813 return -1;
02814 }
02815
02816 extEncoderSetIndexModel(EXT_ENCODER_INDEX_MODEL_MANUAL);
02817 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
02818 {
02819 rawInput[axis] = 0.0;
02820 rawOutput[axis] = 0.0;
02821 trajPos[axis] = 0.0;
02822 jointPos[axis] = 0.0;
02823 oldJointPos[axis] = 0.0;
02824 oldInput[axis] = 0.0;
02825
02826 waitingForLatch[axis] = 0;
02827 latchFlag[axis] = 0;
02828 saveLatch[axis] = 0.0;
02829
02830 wasOnLimit[axis] = 0;
02831 emcmotStatus->input[axis] = 0.0;
02832 lastInput[axis] = 0.0;
02833 emcmotStatus->output[axis] = 0.0;
02834
02835 extAmpEnable(axis,
02836 ! GET_AXIS_ENABLE_POLARITY(axis));
02837 }
02838
02839 emcmotStatus->tail = 0;
02840
02841 #ifdef rtlinux
02842
02843 /* get current time as base for starting tasks */
02844 now = rt_get_time();
02845
02846 /* init control task */
02847 rt_task_init(&emcmotTask, /* RT_TASK * */
02848 emcmotController, /* task code */
02849 0, /* startup arg to task code */
02850 RT_TASK_STACK_SIZE, /* task stack size */
02851 RT_TASK_PRIORITY); /* priority */
02852
02853 /* make sure we save FP context */
02854
02855 #if defined(rtlinux_09J) || defined(rtlinux_1_0)
02856 rt_task_use_fp(&emcmotTask, 1);
02857 #else
02858 rt_use_fp(1);
02859 #endif
02860
02861 /* schedule control task to run */
02862 rt_task_make_periodic(&emcmotTask,
02863 now + SECS_TO_RTIME(0.010),
02864 (RTIME) (RT_TICKS_PER_SEC *
02865 emcmotStatus->servoCycleTime));
02866
02867 #ifdef STEPPER_MOTORS
02868
02869 /* init stepper motor task */
02870 rt_task_init(&smTask, /* RT_TASK * */
02871 smController, /* task code */
02872 0, /* startup arg to task code */
02873 SM_TASK_STACK_SIZE, /* task stack size */
02874 SM_TASK_PRIORITY); /* priority */
02875
02876 /* make sure we save FP context */
02877 #if defined(rtlinux_09J) || defined(rtlinux_1_0)
02878 rt_task_use_fp(&smTask, 1);
02879 #endif
02880
02881 /* schedule stepper task to run at twice servo (axis) rate */
02882 rt_task_make_periodic(&smTask,
02883 now + SECS_TO_RTIME(0.010),
02884 ((RTIME) (RT_TICKS_PER_SEC *
02885 emcmotStatus->servoCycleTime)) >> 1);
02886
02887 #endif
02888
02889 /* FIXME */
02890 diagnostics("inited emcmot\n");
02891
02892 #endif /* rtlinux */
02893
02894 #if defined (rtlinux) && defined (RT_FIFO)
02895 /* create user process handler on control channel */
02896 rtf_create_handler(EMCMOT_COMMAND_RTF, /* fifo number */
02897 emcmotCommandHandler /* control message handler */
02898 );
02899
02900 #endif /* rtlinux and RT_FIFO */
02901 return 0;
02902 }
|
|
||||||||||||||||||||
|
Definition at line 270 of file genhexkins.c. 00274 {
00275 double InverseJacobian[NUM_STRUTS][NUM_STRUTS];
00276 double Jacobian[NUM_STRUTS][NUM_STRUTS];
00277 double velmatrix[6];
00278
00279 if (0 != JInvMat(pos, InverseJacobian)) {
00280 return -1;
00281 }
00282 if (0 != MatInvert(InverseJacobian, Jacobian)) {
00283 return -1;
00284 }
00285
00286 /* Multiply J[] by jointvels to get vels */
00287 MatMult(Jacobian, jointvels, velmatrix);
00288 vel->tran.x = velmatrix[0];
00289 vel->tran.y = velmatrix[1];
00290 vel->tran.z = velmatrix[2];
00291 vel->a = velmatrix[3];
00292 vel->b = velmatrix[4];
00293 vel->c = velmatrix[5];
00294
00295 return 0;
00296 }
|
|
||||||||||||||||||||
|
Definition at line 242 of file genhexkins.c. 00246 {
00247 double InverseJacobian[NUM_STRUTS][NUM_STRUTS];
00248 double velmatrix[6];
00249
00250 if (0 != JInvMat(pos, InverseJacobian)) {
00251 return -1;
00252 }
00253
00254 /* Multiply Jinv[] by vel[] to get jointvels */
00255 velmatrix[0] = vel->tran.x; /* dx/dt */
00256 velmatrix[1] = vel->tran.y; /* dy/dt */
00257 velmatrix[2] = vel->tran.z; /* dz/dt */
00258 velmatrix[3] = vel->a; /* droll/dt */
00259 velmatrix[4] = vel->b; /* dpitch/dt */
00260 velmatrix[5] = vel->c; /* dyaw/dt */
00261 MatMult(InverseJacobian, velmatrix, jointvels);
00262
00263 return 0;
00264 }
|
|
||||||||||||||||||||
|
Definition at line 304 of file genhexkins.c. 00308 {
00309 PmCartesian aw;
00310 PmCartesian InvKinStrutVect,InvKinStrutVectUnit;
00311 PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut;
00312
00313 double Jacobian[NUM_STRUTS][NUM_STRUTS];
00314 double InverseJacobian[NUM_STRUTS][NUM_STRUTS];
00315 double InvKinStrutLength, StrutLengthDiff[NUM_STRUTS];
00316 double delta[NUM_STRUTS];
00317 double conv_err = 1.0;
00318
00319 PmRotationMatrix RMatrix;
00320 PmRpy q_RPY;
00321
00322 int iterate = 1;
00323 int i;
00324 int iteration = 0;
00325 int retval = 0;
00326
00327 #define HIGH_CONV_CRITERION (1e-12)
00328 #define MEDIUM_CONV_CRITERION (1e-5)
00329 #define LOW_CONV_CRITERION (1e-3)
00330 #define MEDIUM_CONV_ITERATIONS 50
00331 #define LOW_CONV_ITERATIONS 100
00332 #define FAIL_CONV_ITERATIONS 150
00333 #define LARGE_CONV_ERROR 10000
00334 double conv_criterion = HIGH_CONV_CRITERION;
00335
00336 /* abort on obvious problems, like joints <= 0 */
00337 /* FIXME-- should check against triangle inequality, so that joints
00338 are never too short to span shared base and platform sides */
00339 if (joints[0] <= 0.0 ||
00340 joints[1] <= 0.0 ||
00341 joints[2] <= 0.0 ||
00342 joints[3] <= 0.0 ||
00343 joints[4] <= 0.0 ||
00344 joints[5] <= 0.0) {
00345 return -1;
00346 }
00347
00348 /* assign a,b,c to roll, pitch, yaw angles */
00349 q_RPY.r = pos->a * PM_PI / 180.0;
00350 q_RPY.p = pos->b * PM_PI / 180.0;
00351 q_RPY.y = pos->c * PM_PI / 180.0;
00352
00353 /* Assign translation values in pos to q_trans */
00354 q_trans.x = pos->tran.x;
00355 q_trans.y = pos->tran.y;
00356 q_trans.z = pos->tran.z;
00357
00358 /* Enter Newton-Raphson iterative method */
00359 while (iterate) {
00360 /* check for large error and return error flag if no convergence */
00361 if ((conv_err > +LARGE_CONV_ERROR) ||
00362 (conv_err < -LARGE_CONV_ERROR)) {
00363 /* we can't converge */
00364 return -2;
00365 };
00366
00367 iteration++;
00368
00369 #if 0
00370 /* if forward kinematics are having a difficult time converging
00371 ease the restrictions on the convergence criterion */
00372 if (iteration == MEDIUM_CONV_ITERATIONS) {
00373 conv_criterion = MEDIUM_CONV_CRITERION;
00374 retval = -3; /* this means if we eventually converge,
00375 the result is sloppy */
00376 }
00377
00378 if (iteration == LOW_CONV_ITERATIONS) {
00379 conv_criterion = LOW_CONV_CRITERION;
00380 retval = -4; /* this means if we eventually converge,
00381 the result is even sloppier */
00382 }
00383 #endif
00384
00385 /* check iteration to see if the kinematics can reach the
00386 convergence criterion and return error flag if it can't */
00387 if (iteration > FAIL_CONV_ITERATIONS) {
00388 /* we can't converge */
00389 return -5;
00390 }
00391
00392 /* Convert q_RPY to Rotation Matrix */
00393 pmRpyMatConvert(q_RPY, &RMatrix);
00394
00395 /* compute StrutLengthDiff[] by running inverse kins on Cartesian
00396 estimate to get joint estimate, subtract joints to get joint deltas,
00397 and compute inv J while we're at it */
00398 for (i = 0; i < NUM_STRUTS; i++) {
00399 pmMatCartMult(RMatrix, a[i], &RMatrix_a);
00400 pmCartCartAdd(q_trans, RMatrix_a, &aw);
00401 pmCartCartSub(aw,b[i], &InvKinStrutVect);
00402 if (0 != pmCartUnit(InvKinStrutVect, &InvKinStrutVectUnit)) {
00403 return -1;
00404 }
00405 pmCartMag(InvKinStrutVect, &InvKinStrutLength);
00406 StrutLengthDiff[i] = InvKinStrutLength - joints[i];
00407
00408 /* Determine RMatrix_a_cross_strut */
00409 pmCartCartCross(RMatrix_a, InvKinStrutVectUnit, &RMatrix_a_cross_Strut);
00410
00411 /* Build Inverse Jacobian Matrix */
00412 InverseJacobian[i][0] = InvKinStrutVectUnit.x;
00413 InverseJacobian[i][1] = InvKinStrutVectUnit.y;
00414 InverseJacobian[i][2] = InvKinStrutVectUnit.z;
00415 InverseJacobian[i][3] = RMatrix_a_cross_Strut.x;
00416 InverseJacobian[i][4] = RMatrix_a_cross_Strut.y;
00417 InverseJacobian[i][5] = RMatrix_a_cross_Strut.z;
00418 }
00419
00420 /* invert Inverse Jacobian */
00421 MatInvert(InverseJacobian, Jacobian);
00422
00423 /* multiply Jacobian by LegLengthDiff */
00424 MatMult(Jacobian, StrutLengthDiff, delta);
00425
00426 /* subtract delta from last iterations pos values */
00427 q_trans.x -= delta[0];
00428 q_trans.y -= delta[1];
00429 q_trans.z -= delta[2];
00430 q_RPY.r -= delta[3];
00431 q_RPY.p -= delta[4];
00432 q_RPY.y -= delta[5];
00433
00434 /* determine value of conv_error (used to determine if no convergence) */
00435 conv_err = 0.0;
00436 for (i = 0; i < NUM_STRUTS; i++) {
00437 conv_err += fabs(StrutLengthDiff[i]);
00438 }
00439
00440 /* enter loop to determine if a strut needs another iteration */
00441 iterate = 0; /*assume iteration is done */
00442 for (i = 0; i < NUM_STRUTS; i++) {
00443 if (fabs(StrutLengthDiff[i]) > conv_criterion) {
00444 iterate = 1;
00445 }
00446 }
00447 } /* exit Newton-Raphson Iterative loop */
00448
00449 /* assign r,p,w to a,b,c */
00450 pos->a = q_RPY.r * 180.0 / PM_PI;
00451 pos->b = q_RPY.p * 180.0 / PM_PI;
00452 pos->c = q_RPY.y * 180.0 / PM_PI;
00453
00454 /* assign q_trans to pos */
00455 pos->tran.x = q_trans.x;
00456 pos->tran.y = q_trans.y;
00457 pos->tran.z = q_trans.z;
00458
00459 return retval;
00460 }
|
|
||||||||||||||||||||
|
Definition at line 500 of file genhexkins.c. 00504 {
00505 *fflags = 0;
00506 *iflags = 0;
00507
00508 return kinematicsInverse(world, joint, iflags, fflags);
00509 }
|
|
||||||||||||||||||||
|
Definition at line 464 of file genhexkins.c. 00468 {
00469 PmCartesian aw, temp;
00470 PmRotationMatrix RMatrix;
00471 PmRpy rpy;
00472 int i;
00473
00474 /* define Rotation Matrix */
00475 rpy.r = pos->a * PM_PI / 180.0;
00476 rpy.p = pos->b * PM_PI / 180.0;
00477 rpy.y = pos->c * PM_PI / 180.0;
00478 pmRpyMatConvert(rpy, &RMatrix);
00479
00480 /* enter for loop to calculate joints (strut lengths) */
00481 for (i = 0; i < NUM_STRUTS; i++) {
00482 /* convert location of platform strut end from platform
00483 to world coordinates */
00484 pmMatCartMult(RMatrix, a[i], &temp);
00485 pmCartCartAdd(pos->tran, temp, &aw);
00486
00487 /* define strut lengths */
00488 pmCartCartSub(aw, b[i], &temp);
00489 pmCartMag(temp, &joints[i]);
00490 }
00491
00492 return 0;
00493 }
|
|
|
|
|
|
Definition at line 511 of file genhexkins.c. Referenced by init_module().
00512 {
00513 #if 1
00514 return KINEMATICS_BOTH;
00515 #else
00516 return KINEMATICS_INVERSE_ONLY;
00517 #endif
00518 }
|
1.2.11.1 written by Dimitri van Heesch,
© 1997-2001