Hello everyone,
I have an 6 dof anthropomorphic robot like Puma but without spherical wirst. I
have written my new kinematic editing pumakins.c and using the standard
equations for a anthropomorphic manipulator (3 DOF) plus direct expressions
like a trivial kinematics for the lastest 3 DOF. I wanted to separate 3 and 3
dof.
Now when I run emc2 and I switch on world mode, I have a "following error" and
the software is stopped. It seems that the robot searches some position without
that I move the robot.
Which could be the problem? I think that it may be a conflict between home and
inverse kinematics. How they relate?
I attached my files here.
Thanks
Francesca
/*****************************************************************
* Description: armkins.c
* Kinematics for antropomorphic manipulator 3R
*
*
* Author: Francesca
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
*******************************************************************
*/
#include "rtapi_math.h"
#include "posemath.h"
#include "armkins.h"
#include "kinematics.h" /* decls for kinematicsForward, etc. */
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
int kinematicsForward(const double * joint,
EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double s1, s2, s3, s4, s5, s6;
double c1, c2, c3, c4, c5, c6;
double s23;
double c23;
double sumSq, k;
PmHomogeneous hom;
PmPose worldPose;
/* Calculate sin of joints for future use */
s1 = sin(joint[0]*PM_PI/180.0);
s2 = sin(joint[1]*PM_PI/180.0);
s3 = sin(joint[2]*PM_PI/180.0);
s4 = sin(joint[3]*PM_PI/180.0);
s5 = sin(joint[4]*PM_PI/180.0);
s6 = sin(joint[5]*PM_PI/180.0);
/* Calculate cos of joints for future use */
c1 = cos(joint[0]*PM_PI/180.0);
c2 = cos(joint[1]*PM_PI/180.0);
c3 = cos(joint[2]*PM_PI/180.0);
c4 = cos(joint[3]*PM_PI/180.0);
c5 = cos(joint[4]*PM_PI/180.0);
c6 = cos(joint[5]*PM_PI/180.0);
s23 = c2 * s3 + s2 * c3;
c23 = c2 * c3 - s2 * s3;
/* Dimensione dei links L1=245.0 L2=190.0 L3=190.0 */
/* Definizione della prima colonna della matrice di rotazione */
hom.rot.x.x = c1 * c23;
hom.rot.x.y = s1 * c23;
hom.rot.x.z = s23;
/* Definizione della seconda colonna della matrice di rotazione */
hom.rot.y.x = -c1 * s23;
hom.rot.y.y = -s1 * s23;
hom.rot.y.z = c23;
/* Definizione della terza colonna della matrice di rotazione */
hom.rot.z.x = s1;
hom.rot.z.y = -c1;
hom.rot.z.z = 0.0;
/* Definizione della posizione del vettore */
hom.tran.x = c1 * (190.0 * c2 + 190.0 * c23);
hom.tran.y = s1 * (190.0 * c2 + 190.0 * c23);
hom.tran.z = 190.0 * s2 + 190.0 * s23 + 245.0;
/* Definizione delle singolarità e delle flags */
sumSq = (hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y + hom.tran.z *
hom.tran.z - 190.0 * 190.0 - 190.0 * 190.0 ) / (2.0 * 190.0 * 190.0);
k = sqrt(1.0 - sumSq * sumSq);
/* reset flags */
*iflags = 0;
/* Setta la singolarità di spalla */
if (fabs(joint[0]*PM_PI/180.0 - atan2(hom.tran.y, hom.tran.x)) < FLAG_FUZZ)
{
*iflags |= SINGOL_SPALLA;
}
/* Setta la singolarità di gomito */
if (fabs(joint[2]*PM_PI/180 - atan2(sumSq, k)) < FLAG_FUZZ)
{
*iflags |= SINGOL_GOMITO;
}
/* convert hom.rot to world->quat */
pmHomPoseConvert(hom, &worldPose);
world->tran = worldPose.tran;
world->a = joint[3];
world->b = joint[4];
world->c = joint[5];
/* return 0 and exit */
return 0;
}
int kinematicsInverse(const EmcPose * world,
double * joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
PmHomogeneous hom;
PmPose worldPose;
int singular;
double t1, t2, t3;
double k;
double sumSq;
double th1;
double th3;
double th23;
double th2;
double th4;
double th5;
double th6;
double s1, c1;
double s2, c2;
double s3, c3;
double s23, c23;
double s4, c4;
double s5, c5;
double s6, c6;
/* reset flags */
*fflags = 0;
/* convert pose to hom */
worldPose.tran = world->tran;
joint[3] = world->a;
joint[4] = world->b;
joint[5] = world->c;
pmPoseHomConvert(worldPose, &hom);
/* Joint 1 (2 independent solutions) */
if (*iflags & SINGOL_SPALLA){
th1 = atan2(hom.tran.y, hom.tran.x);
}
else{
th1 = PM_PI + atan2(hom.tran.y, hom.tran.x);
}
/* save sin, cos for later calcs */
s1 = sin(th1);
c1 = cos(th1);
/* Joint 3 (2 independent solutions) */
c3 = (hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y + hom.tran.z *
hom.tran.z - 190.0 * 190.0 - 190.0 * 190.0 ) / (2.0 * 190.0 * 190.0);
if (*iflags & SINGOL_GOMITO){
s3 = sqrt(1.0 - c3 * c3);
}
else{
s3 = -sqrt(1.0 - c3 * c3);
}
th3 = atan2(s3,c3);
/* Joint 2 */
s2 = ((190.0 + 190.0 * c3) * hom.tran.z - 190.0 * s3 * sqrt(hom.tran.x *
hom.tran.x + hom.tran.y * hom.tran.y)) / (hom.tran.x * hom.tran.x + hom.tran.y
* hom.tran.y + hom.tran.z * hom.tran.z);
c2 = ((190.0 + 190.0 * c3) * sqrt(hom.tran.x * hom.tran.x + hom.tran.y *
hom.tran.y) + 190.0 * s3 * hom.tran.z) / (hom.tran.x *hom.tran.x + hom.tran.y *
hom.tran.y + hom.tran.z * hom.tran.z);
th2 = atan2(s2,c2);
/* copy out */
joint[0] = th1*180.0/PM_PI;
joint[1] = th2*180.0/PM_PI;
joint[2] = th3*180.0/PM_PI;
return 0;
}
int kinematicsHome(EmcPose * world,
double * joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
/* use joints, set world */
return kinematicsForward(joint, world, fflags, iflags);
}
KINEMATICS_TYPE kinematicsType()
{
// return KINEMATICS_FORWARD_ONLY;
return KINEMATICS_BOTH;
}
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
int comp_id;
int rtapi_app_main(void) {
comp_id = hal_init("armkins");
if(comp_id > 0) {
hal_ready(comp_id);
return 0;
}
return comp_id;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
/*****************************************************************
* Description: armkins.h
* Kinematics for armdroid robot
*
* D
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
*******************************************************************
* This is the header file to accompany armkins.c.
*******************************************************************
*/
#ifndef ARM_H
#define ARM_H
#define SINGULAR_FUZZ 0.000001
#define FLAG_FUZZ 0.000001
/* flags for inverse kinematics */
#define SINGOL_SPALLA 0x01
#define SINGOL_GOMITO 0x02
#endif /* ARM_H */
------------------------------------------------------------------------------
Keep Your Developer Skills Current with LearnDevNow!
The most comprehensive online learning library for Microsoft developers
is just $99.99! Visual Studio, SharePoint, SQL - plus HTML5, CSS3, MVC3,
Metro Style Apps, more. Free future releases when you subscribe now!
http://p.sf.net/sfu/learndevnow-d2d
_______________________________________________
Emc-developers mailing list
Emc-developers@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/emc-developers
