You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
245 lines
6.2 KiB
245 lines
6.2 KiB
#ifndef _LWR4_H_
|
|
#define _LWR4_H_
|
|
|
|
#include "Vec.h"
|
|
#include "Mat.h"
|
|
#include "friComm.h"
|
|
#include "Robot.h"
|
|
|
|
|
|
class LWR4 : public Robot
|
|
{
|
|
public :
|
|
const static int joints = 7;
|
|
typedef Vec<float,joints> VecJoint;
|
|
|
|
|
|
static VecJoint getJointRange(void);
|
|
static VecJoint getJointVelocity(void);
|
|
static VecJoint getJointAcceleration(void);
|
|
|
|
static Vec<float, 4> getDhParameter(unsigned int joint);
|
|
|
|
static VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config );
|
|
static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config );
|
|
};
|
|
|
|
Robot::VecJoint LWR4::backwardCalc(MatCarthesian M , struct RobotConfig config )
|
|
{
|
|
Robot::VecJoint Joints(0.0f);
|
|
|
|
//DH-Parameter
|
|
float alp[7] = {-M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, 0.0f};
|
|
float d[7] = {310.4f/1000.0f, 0.0f, 400.1f/1000.0f, 0.0f, 390.0f/1000.0f, 0.0f, 78.0f/1000.0f};
|
|
//Parameter
|
|
int ELBOW = (config.elbow ? 1.0f : -1.0f );
|
|
int FLIP = (config.flip ? 1.0f : -1.0f );
|
|
float J1os = (config.j1os ? 1.0f : -1.0f )/180.0*M_PI;
|
|
|
|
//Calculating M06
|
|
MatCarthesian R67;
|
|
int i = 6;
|
|
|
|
R67(0,0) = cos(0);
|
|
R67(0,1) = -sin(0)*cos(alp[i]);
|
|
R67(0,2) = sin(0)*sin(alp[i]);
|
|
R67(0,3) = 0.0f;
|
|
|
|
R67(1,0) = sin(0);
|
|
R67(1,1) = cos(0)*cos(alp[i]);
|
|
R67(1,2) = -cos(0)*sin(alp[i]);
|
|
R67(1,3) = 0.0f;
|
|
|
|
R67(2,0) = 0.0f;
|
|
R67(2,1) = sin(alp[i]);
|
|
R67(2,2) = cos(alp[i]);
|
|
R67(2,3) = d[i];
|
|
|
|
R67(3,0) = 0.0f;
|
|
R67(3,1) = 0.0f;
|
|
R67(3,2) = 0.0f;
|
|
R67(3,3) = 1.0f;
|
|
|
|
MatCarthesian M06;
|
|
MatCarthesian Inv;
|
|
Inv = R67.inv();
|
|
M06=M*Inv;
|
|
|
|
//Joint 1 including given offset
|
|
Joints(0) = atan2(M06(1,3),M06(0,3))+J1os;
|
|
|
|
//Calculating M16
|
|
MatCarthesian R01;
|
|
i = 0;
|
|
R01(0,0) = cos(Joints(i));
|
|
R01(0,1) = -sin(Joints(i))*cos(alp[i]);
|
|
R01(0,2) = sin(Joints(i))*sin(alp[i]);
|
|
R01(0,3) = 0.0f;
|
|
|
|
R01(1,0) = sin(Joints(i));
|
|
R01(1,1) = cos(Joints(i))*cos(alp[i]);
|
|
R01(1,2) = -cos(Joints(i))*sin(alp[i]);
|
|
R01(1,3) = 0.0f;
|
|
|
|
R01(2,0) = 0.0f;
|
|
R01(2,1) = sin(alp[i]);
|
|
R01(2,2) = cos(alp[i]);
|
|
R01(2,3) = d[i];
|
|
|
|
R01(3,0) = 0.0f;
|
|
R01(3,1) = 0.0f;
|
|
R01(3,2) = 0.0f;
|
|
R01(3,3) = 1.0f;
|
|
|
|
Inv = R01.inv();
|
|
MatCarthesian M16;
|
|
M16 = Inv*M06;
|
|
|
|
//Joint 2
|
|
float m14 = M16(0,3);
|
|
float m24 = M16(1,3);
|
|
float m34 = M16(2,3);
|
|
float d3 = d[2];
|
|
float d5 = d[4];
|
|
|
|
float arg1 = (0.5*(ELBOW*m24*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14
|
|
-ELBOW*m24*m24*m24+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3
|
|
+2*m14*m14*m14*m14*d5*d5+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14
|
|
-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14
|
|
+2*m24*m24*d3*d3*m14*m14-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34
|
|
+2*m14*m14*d5*d5*d3*d3+2*m24*m24*d5*d5*m14*m14))/(m24*m24+m14*m14)
|
|
+m34*m34+d3*d3-1*d5*d5+m14*m14+m24*m24))/(d3*m14);
|
|
|
|
float arg2 = ELBOW*(0.5*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14-ELBOW*m24*m24*m24
|
|
+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3+2*m14*m14*m14*m14*d5*d5
|
|
+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24
|
|
*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14+2*m24*m24*d3*d3*m14*m14
|
|
-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34+2*m14*m14*d5*d5*d3*d3
|
|
+2*m24*m24*d5*d5*m14*m14)))/((m24*m24+m14*m14)*d3);
|
|
|
|
Joints(1) = atan2(arg1,arg2);
|
|
|
|
//Calculating M26
|
|
MatCarthesian R12;
|
|
i = 1;
|
|
|
|
R12(0,0) = cos(Joints(i));
|
|
R12(0,1) = -sin(Joints(i))*cos(alp[i]);
|
|
R12(0,2) = sin(Joints(i))*sin(alp[i]);
|
|
R12(0,3) = 0.0f;
|
|
|
|
R12(1,0) = sin(Joints(i));
|
|
R12(1,1) = cos(Joints(i))*cos(alp[i]);
|
|
R12(1,2) = -cos(Joints(i))*sin(alp[i]);
|
|
R12(1,3) = 0.0f;
|
|
|
|
R12(2,0) = 0.0f;
|
|
R12(2,1) = sin(alp[i]);
|
|
R12(2,2) = cos(alp[i]);
|
|
R12(2,3) = d[i];
|
|
|
|
R12(3,0) = 0.0f;
|
|
R12(3,1) = 0.0f;
|
|
R12(3,2) = 0.0f;
|
|
R12(3,3) = 1.0f;
|
|
|
|
Mat4f R02;
|
|
R02 = R01*R12;
|
|
Inv = R02.inv();
|
|
Mat4f M26;
|
|
M26 = Inv*M06;
|
|
|
|
//Joint 3
|
|
Joints(2) = atan2(ELBOW*M26(1,3),ELBOW*M26(0,3));
|
|
|
|
//Calculating M36
|
|
MatCarthesian R23;
|
|
i = 2;
|
|
|
|
R23(0,0) = cos(Joints(i));
|
|
R23(0,1) = -sin(Joints(i))*cos(alp[i]);
|
|
R23(0,2) = sin(Joints(i))*sin(alp[i]);
|
|
R23(0,3) = 0.0f;
|
|
|
|
R23(1,0) = sin(Joints(i));
|
|
R23(1,1) = cos(Joints(i))*cos(alp[i]);
|
|
R23(1,2) = -cos(Joints(i))*sin(alp[i]);
|
|
R23(1,3) = 0.0f;
|
|
|
|
R23(2,0) = 0.0f;
|
|
R23(2,1) = sin(alp[i]);
|
|
R23(2,2) = cos(alp[i]);
|
|
R23(2,3) = d[i];
|
|
|
|
R23(3,0) = 0.0f;
|
|
R23(3,1) = 0.0f;
|
|
R23(3,2) = 0.0f;
|
|
R23(3,3) = 1.0f;
|
|
|
|
MatCarthesian R03;
|
|
R03 = R01*R12*R23;
|
|
Inv = R03.inv();
|
|
MatCarthesian M36;
|
|
M36 = Inv*M06;
|
|
|
|
//Joint 4
|
|
Joints(3) = atan2(M36(0,3),-M36(1,3));
|
|
|
|
//Calculating M47
|
|
MatCarthesian R34;
|
|
i = 3;
|
|
R34(0,0) = cos(Joints(i));
|
|
R34(0,1) = -sin(Joints(i))*cos(alp[i]);
|
|
R34(0,2) = sin(Joints(i))*sin(alp[i]);
|
|
R34(0,3) = 0.0f;
|
|
|
|
R34(1,0) = sin(Joints(i));
|
|
R34(1,1) = cos(Joints(i))*cos(alp[i]);
|
|
R34(1,2) = -cos(Joints(i))*sin(alp[i]);
|
|
R34(1,3) = 0.0f;
|
|
|
|
R34(2,0) = 0.0f;
|
|
R34(2,1) = sin(alp[i]);
|
|
R34(2,2) = cos(alp[i]);
|
|
R34(2,3) = d[i];
|
|
|
|
R34(3,0) = 0.0f;
|
|
R34(3,1) = 0.0f;
|
|
R34(3,2) = 0.0f;
|
|
R34(3,3) = 1.0f;
|
|
|
|
MatCarthesian R04;
|
|
R04 = R01*R12*R23*R34;
|
|
Inv = R04.inv();
|
|
MatCarthesian M47;
|
|
M47 = Inv*M;
|
|
|
|
//Joint 5
|
|
float th = 0.0001;
|
|
if (abs(M47(1,2))<th && abs(M47(0,2))<th)
|
|
Joints(4) = 0.0f;
|
|
else
|
|
Joints(4) = atan2(FLIP*M47(1,2),FLIP*M47(0,2));
|
|
|
|
//Joint 6
|
|
Joints(5) = atan2(FLIP*sqrt(1-M47(2,2)*M47(2,2)),M47(2,2));
|
|
|
|
//Joint 7
|
|
if (abs(M47(2,1))<th && abs(M47(2,0))<th)
|
|
Joints(6) = 0.0f;
|
|
else
|
|
Joints(6) = atan2(FLIP*M47(2,1),FLIP*-M47(2,0));
|
|
|
|
for (int i = 0; i<7; i++)
|
|
{
|
|
Joints(i) = Joints(i)*180.0/M_PI;
|
|
}
|
|
|
|
//Kuka Joints
|
|
//Joints[3] = -Joints[3];
|
|
Joints(1) = -Joints(1);
|
|
Joints(5) = -Joints(5);
|
|
|
|
return Joints;
|
|
}
|
|
#endif
|