#ifndef _LWR4_H_ #define _LWR4_H_ #include "Robot.h" /** * @addtogroup robot * @{ * @addtogroup lwr4 * @{ * @author Philipp Schoenberger * @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 : LWR4(); Robot::VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config ); MatCarthesian forwardCalc(Robot::VecJoint, struct RobotConfig config ); }; /** * default constructor for the lwr4 robot which is seting up all constraints */ LWR4::LWR4() { 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))