phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
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.
65 Commits
1 Branch
0 Tags
31 MiB
 
 
 
 
 
 
Tree: 0ba213938e
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '0ba213938e'
${ noResults }
lwr_server/lwrserv/include/lwr4.h

299 lines
8.1 KiB
Raw Blame History

#ifndef _LWR4_H_
#define _LWR4_H_
#include "Vec.h"
#include "Mat.h"
#include "friComm.h"
#include "Robot.h"
/**
* @addtogroup robot
* @{
* @addtogroup lwr4
* @{
* @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
* @version 1.0
*
* @section LICENSE
*
* 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 at
* https://www.gnu.org/copyleft/gpl.html
*
* @section DESCRIPTION
*
* This file contains the calculation functions for the lwr4 robot
*
* The slowest joint is defining the speed of the other joints.
* By that all joints start and stop the movement synchronously
*/
/**
* Class for a LWR4 robot
* The joint start and stop synchron
*
* This class is based on the Robot class
*
* @see Robot
*/
class LWR4 : public Robot
{
// the LWR4 robot has 7 Joints
// use the macro from fri remote
public :
unsigned int joints;
LWR4();
LWR4::VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config );
MatCarthesian forwardCalc(Robot::VecJoint, struct RobotConfig config );
};
LWR4::LWR4()
{
joints = LBR_MNJ;
float jointRangeMax_[] = { 170.0f, 120.0f, 170.0f, 120.0f, 170.0f, 120.0f, 170.0f};
float jointRangeMin_[] = {-170.0f,-120.0f,-170.0f,-120.0f,-170.0f,-120.0f,-170.0f};
float jointVelocity_[] = { 110.0f, 110.0f, 128.0f, 128.0f, 204.0f, 184.0f, 184.0f};
float jointAcceleration_[] = { 1.0f, 1.0f, 1.0f, 1.0f, 2.0f, 1.0f, 1.0f};
float jointTorque_[] = { 176.0f, 176.0f, 100.0f, 100.0f, 100.0f, 38.0f, 38.0f};
this->jointRangeMin = VecJoint(jointRangeMin_);
this->jointRangeMax = VecJoint(jointRangeMax_);
this->jointVelocity = VecJoint(jointVelocity_);
this->jointAcceleration = VecJoint(jointAcceleration_);
this->jointTorque = VecJoint(jointTorque_);
}
/**
* Calculates an vector for the joint angles for a given carthesian Position.
*
* @param M homogenious matrix for the Position
* @param config the robot configuration like elbow etc.
* @return the angles if the position could be reached.
*/
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