diff --git a/lwrserv/include/BangBangTrajectory.h b/lwrserv/include/BangBangTrajectory.h index fec3920..53ac43a 100644 --- a/lwrserv/include/BangBangTrajectory.h +++ b/lwrserv/include/BangBangTrajectory.h @@ -3,10 +3,61 @@ #include "Trajectroy.h" #include "Vec.h" +/** + * @addtogroup trajectory + * @{ + * + * @addtogroup BangBang + * @{ + * @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 Trajectory for a bang bang trajectory. + * The bang bang trajectory is a trajectory with linear acceleration + * phase followed by a direct de-acceleration phase + * + * The slowest joint is defining the speed of the other joints. + * By that all joints start and stop the movement synchronously + */ template + +/** + * Class for a Bang Bang Trajectory based on joint angles. + * The joint start and stop synchron + * + * This class is based on the Trajectory joint Template so it + * all possible joint counts + * + * @see Trajectory + */ class BangBangJointTrajectory : public Trajectory { public: + /** + * standard constructor which will setup the class for a + * trajectory from start to end position with a synchron movement + * + * @param sampleTimeMs The discrete time interval for each step within the Trajectory with the unit [ milliseconds ] + * @param maxJointVelocity The maximum velocity for each joint with the unit [ degree/ seconds ] + * @param maxJointAcceleration The maximum acceleration for each joint with the unit [ degree/(seconds^2) ] + * @param jointStart The starting point for each joint in [ degree ] + * @param jointEnd The point which should be reached at the end of the Trajectory [ degree ] + */ BangBangJointTrajectory( float sampleTimeMs, Vec maxJointVelocity, @@ -15,7 +66,6 @@ class BangBangJointTrajectory : public Trajectory Vec jointEnd ) { - std::cout << "bang bang \n " ; float MStoSec = 1000.0f; float sampleTime = sampleTimeMs / MStoSec; // calculate maximum velocity and acceleration @@ -31,7 +81,7 @@ class BangBangJointTrajectory : public Trajectory // calculate number of movement steps // one joint has to reach maxvelocity the others are stepped down to - // calculate time if acceleration is enouth to reach max speed + // calculate time if acceleration is enough to reach max speed // s = a * t^2 / 2 Vec minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime; //TODO check if mintime is neceesary @@ -52,8 +102,9 @@ class BangBangJointTrajectory : public Trajectory Vec velocityLast(0.0f); // percentage of max velocity - //Vec currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ; - // s = a* t^2 / 2 => a = s* 2 / t^2 + // s = a* t^2 / 2 + // <=> + // a = s* 2 / t^2 Vec currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f; std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n"; @@ -66,7 +117,7 @@ class BangBangJointTrajectory : public Trajectory this->nodes[i].acceleration = currMaxAcceleration ; count +=1.0f; } - // deacceleration phase + // de-acceleration phase else { this->nodes[i].acceleration = currMaxAcceleration * -1.0f; @@ -74,20 +125,36 @@ class BangBangJointTrajectory : public Trajectory } this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count; this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; - + + // check if we already reached end + // This can happen if the steps calculation was + // to far off caused by floating point precision + + float maxdiffLast = (jointEnd - jointLast).abs().max(); + float maxdiffCurr = (jointEnd - this->nodes[i].jointPos).abs().max(); + if ( maxdiffLast < maxdiffCurr) + { + this->steps = i+1; + + this->nodes[i].jointPos = jointEnd; + break; + } + + // save last joints jointLast = this->nodes[i].jointPos; velocityLast = this->nodes[i].velocity; } } - private: - - protected: - }; template class BangBangCartTrajectory: public Trajectory { }; +/** + * @} + * @} + */ + #endif diff --git a/lwrserv/include/commands.h b/lwrserv/include/commands.h index 464fa2d..e8416bf 100644 --- a/lwrserv/include/commands.h +++ b/lwrserv/include/commands.h @@ -4,6 +4,14 @@ #include "SocketObject.h" #include +/** + * @addtogroup commands + * functions to process the request from client side and respond to them + * @{ + * @addtogroup processing + * @{ + */ + static struct ClientCommand* commands; static unsigned int commandCount; @@ -39,4 +47,9 @@ void quit(SocketObject& client, std::string& arg); // check if we use a kuka void isKukaLwr(SocketObject& client, std::string& arg); +/** + * @} + * @} + */ + #endif diff --git a/lwrserv/include/commandsHelp.h b/lwrserv/include/commandsHelp.h new file mode 100644 index 0000000..2e0696d --- /dev/null +++ b/lwrserv/include/commandsHelp.h @@ -0,0 +1,45 @@ +#ifndef _COMMANDSHELP_H_ +#define _COMMANDSHELP_H_ + +#include "SocketObject.h" +#include +/** + * @addtogroup commands + * Additional functions for the client commands to print help and usage of them + * @{ + * @addtogroup help + * @{ + */ + +//Handling request for current Joint Values +void getPositionJointsHelp (SocketObject& client, std::string& arg); +//Get Position as POSE Matrix +void getPositionHomRowWiseHelp (SocketObject& client, std::string& arg); +//Get Force/torque values from TCP +void getForceTorqueTcpHelp (SocketObject& client, std::string& arg); +//Move to given Joint combination +void movePTPJointsHelp (SocketObject& client, std::string& arg); +//Move to given POSE position +void moveHomRowWiseStatusHelp (SocketObject& client, std::string& arg); +//Set Velocity +void setSpeedHelp (SocketObject& client, std::string& arg); +//Set Acceleration +void setAccelHelp (SocketObject& client, std::string& arg); +//Starting Potential Field Movement Mode +void startPotFieldModeHelp (SocketObject& client, std::string& arg); +//Stopping Potential Field Movement Mode +void stopPotFieldModeHelp (SocketObject& client, std::string& arg); +// set the current trajectory type +void setTrajectoryTypeHelp (SocketObject& client, std::string& arg); +// get the current trajectory type +void getTrajectoryTypeHelp (SocketObject& client, std::string& arg); +//Quit +void quitHelp (SocketObject& client, std::string& arg); +// check if we use a kuka +void isKukaLwrHelp (SocketObject& client, std::string& arg); + +/** + * @} + * @} + */ +#endif diff --git a/lwrserv/include/lwr4.h b/lwrserv/include/lwr4.h index 40d245a..9e263b9 100644 --- a/lwrserv/include/lwr4.h +++ b/lwrserv/include/lwr4.h @@ -1,6 +1,5 @@ #ifndef _LWR4_H_ #define _LWR4_H_ - #include "Vec.h" #include "Mat.h" #include "friComm.h" @@ -14,10 +13,10 @@ class LWR4 : public Robot static Robot::VecJoint getJointVelocity(void); static Robot::VecJoint getJointAcceleration(void); - static Vec getDhParameter(unsigned int joint); + static Vec getDhParameter(unsigned int joint); - static Robot::VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config ); - static MatCarthesian forwardCalc(Robot::VecJoint, struct RobotConfig config ); + static Robot::VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config ); + static MatCarthesian forwardCalc(Robot::VecJoint, struct RobotConfig config ); }; Robot::VecJoint LWR4::backwardCalc(MatCarthesian M , struct RobotConfig config ) diff --git a/lwrserv/src/commands.cpp b/lwrserv/src/commands.cpp index 3932185..4f99f41 100644 --- a/lwrserv/src/commands.cpp +++ b/lwrserv/src/commands.cpp @@ -18,7 +18,8 @@ * * @section DESCRIPTION * - * This file contains the commands for the client interface + * This file contains functions to process the commands + * comming from the client interface */ #include #include @@ -68,6 +69,16 @@ void moveRobotTo(Robot::VecJoint newJointPos) boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) ); } } + +/** + * This function processes the request from client side to + * get the current joint positions of the robot + * + * @param client connection to client which will recive the response + * @param arg unused but has to be there for matching the signature + * + * @see getPositionJointsHelp + */ void getPositionJoints(SocketObject& client, std::string& arg) { // unused @@ -80,7 +91,7 @@ void getPositionJoints(SocketObject& client, std::string& arg) Robot::VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); // assemble command feadback - for (int i=0; i< Robot::joints; i++) + for (unsigned int i = 0 ; i < Robot::joints ; i++) { ss.str(""); ss << (jointPos(i)*180/M_PI); @@ -91,11 +102,21 @@ void getPositionJoints(SocketObject& client, std::string& arg) client.Send(out); } +/** + * This function processes the request from client side to + * get the current homogenious carthesian coordinates + * + * @param client connection to client which will recive the response + * @param arg unused but has to be there for matching the signature + * + * @see getPositionHomRowWiseHelp + */ void getPositionHomRowWise(SocketObject& client, std::string& arg) { // unused (void) arg; + // set up the string buffers std::string out = ""; std::stringstream ss (std::stringstream::in | std::stringstream::out); @@ -103,52 +124,79 @@ void getPositionHomRowWise(SocketObject& client, std::string& arg) MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos(); // assemble command feedback row wise - for (int i=0; i< FRI_CART_FRM_DIM; i++) + // leav out the 4th row cause its containing only " 0 0 0 1 " + for (int row = 0; row < 3 ; row++) { - int row = i % 4; - int column = i / 4; - - ss.str(""); - ss << (cartPos(row,column)); - out += ss.str() + " "; + for (int column = 0; column < 4 ; column++) + { + ss.str(""); + ss << (cartPos(row,column)); + out += ss.str() + " "; + } } - + // send msg to client client.Send(out); } +/** + * This function processes the request from client side to + * get the current force and torque based on the endefector + * of the robot + * + * @param client connection to client which will recive the response + * @param arg unused but has to be there for matching the signature + * + * @see getForceTorqueTcpHelp + */ void getForceTorqueTcp(SocketObject& client, std::string& arg) { // unused (void) arg; + // set up the string buffers std::string out = ""; std::stringstream ss (std::stringstream::in | std::stringstream::out); + // get current force and torque parametes from database VecTorque torque = SvrData::getInstance()->getMessuredForceTorque(); + + // assemble command feedback for (int i=0; i< FRI_CART_VEC; i++) { ss.str(""); ss << torque(i); out += ss.str() + " "; } + + // send msg to client client.Send(out); } +/** + * This function processes the request from client side to + * move the robot from the current position to a + * new position defined by the new joint angles + * + * @param client connection to client which will recive the response + * @param arg array containg the new joint angles from fist to last one + * + * @see movePTPJointsHelp + */ void movePTPJoints(SocketObject& client, std::string& arg) { + // set up the string buffers std::string buf; - std::stringstream ss(arg); std::stringstream ss2f; // convert string to joint vector Robot::VecJoint newJointPos(0.0f); - int i = 0; + unsigned int i = 0; while (ss >> buf) { // to many joint values - if (i>=Robot::joints) + if (i >= Robot::joints) { client.Send(SVR_INVALID_ARGUMENT_COUNT); return; @@ -161,10 +209,10 @@ void movePTPJoints(SocketObject& client, std::string& arg) } // to less joint values - if (i!=Robot::joints) + if (i != Robot::joints) { client.Send(SVR_INVALID_ARGUMENT_COUNT); - return ; + return; } //check for joint range @@ -175,12 +223,24 @@ void movePTPJoints(SocketObject& client, std::string& arg) return; } + // to actually move the robot to new joint angles moveRobotTo(newJointPos); client.Send(SVR_TRUE_RSP); } +/** + * This function processes the request from client side to + * move the robot from the current position to a + * new position defined by the new homogeneous coordinates and configuration parameters + * + * @param client connection to client which will receive the response + * @param arg array containing the new carthesian positions and configuration parameters (elbow, flip, j1os) + * + * @see moveHomRowWiseStatusHelp + */ void moveHomRowWiseStatus(SocketObject& client, std::string& arg) { + // set up the string buffers std::string buf; double temp[15]; std::stringstream ss(arg); @@ -223,9 +283,9 @@ void moveHomRowWiseStatus(SocketObject& client, std::string& arg) // extract elbow flip and hand parameter struct Robot::RobotConfig configurationParameter; - configurationParameter.elbow = temp[12] == 1.0f; - configurationParameter.flip = temp[13] == 1.0f; - configurationParameter.j1os = temp[14] == 1.0f; + configurationParameter.elbow = (temp[12] == 1.0f); + configurationParameter.flip = (temp[13] == 1.0f); + configurationParameter.j1os = (temp[14] == 1.0f); //Backward Calculation Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); @@ -243,6 +303,15 @@ void moveHomRowWiseStatus(SocketObject& client, std::string& arg) client.Send(SVR_TRUE_RSP); } +/** + * This function processes the request from client side to + * set a new percentage for the speed value. + * + * @param client connection to client which will receive the response + * @param arg new percentage + * + * @see moveHomRowWiseStatusHelp + */ void setSpeed(SocketObject& client, std::string& arg) { std::string buf; @@ -391,17 +460,15 @@ void getTrajectoryType(SocketObject& client, std::string& arg) } /** - * clients can check if the robot is of Type Kuka LWR + * * @param client connection to client which will recive the response * @param arg aguments which are unused in this case - * - * @author Philipp Schoenberger - * @date */ void isKukaLwr(SocketObject& client, std::string& arg) { //unused parameter (void) arg; + // we are a kuka so send true client.Send(SVR_TRUE_RSP); } diff --git a/lwrserv/src/commandsHelp.cpp b/lwrserv/src/commandsHelp.cpp new file mode 100644 index 0000000..2d8bf10 --- /dev/null +++ b/lwrserv/src/commandsHelp.cpp @@ -0,0 +1,336 @@ +/** + * @file + * @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 functions to process the commands + * coming from the client interface + */ +#include +#include +#include +#include "SocketObject.h" + +/** + * This function returns the help information text to the client + * for the getPositionJoints function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see getPositionJoints + */ +void getPositionJointsHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // add the help text to the buffer + out += "Request for the current joint angles from database.\n"; + out += "Return string is formated like: \n \t 1.0 2.0 ... 4.0 \n "; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the getPositionHomRowWise function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see getPositionHomRowWise + */ +void getPositionHomRowWiseHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // add the help text to the buffer + out += "Request for the current positions in form of a homogeneous cartesian matrix.\n"; + out += "The return string is formated row wise with leaving out the last line.\n"; + out += "\n"; + out += "R11 R12 R13 Tx \n"; + out += "R21 R22 R23 Ty \n"; + out += "R31 R32 R33 Tz \n"; + out += "0 0 0 1 \n"; + out += "\n"; + out += "Example Matrix above will be assembled to a line like the following one\n"; + out += "\n"; + out += "R11 R12 R13 Tx R21 R22 R23 Ty R31 R32 R33 Tz\n"; + out += "\n"; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the getForceTorqueTcp function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see getForceTorqueTcp + */ +void getForceTorqueTcpHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // add the help text to the buffer + out += "Request for the current force and torque of the Robot\n"; + out += "The first 3 Values are the x,y,z forces of the robot endefektor\n"; + out += "The second 3 values are the x,y,z rotational torques on the endefektor\n"; + out += "The returned string will be formated like this: \n"; + out += "\t Fx Fy Fz Tx Ty Tz\n"; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the movePTPJoints function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see movePTPJoints + */ +void movePTPJointsHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // add the help text to the buffer + out += "Request the server to move the robot from the current measured Position\n"; + out += "to the new position defined by the given parameters.\n"; + out += "The amount of parameters have to match exactly the number of joints of the\n"; + out += "Robot. Otherwise the command will return \"false\".\n"; + out += "Command interface is blocked after sending the command and will be up\n"; + out += "when the robot reached the commanded destination.\n"; + out += "When the movement was successfully finished the return value will be \"true\"\n"; + out += "The entered command should be formated like this: \n"; + out += "MPTP J1 J2 J3 J4 J5 J6 J7 ... \n"; + + // send msg to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the moveHomRowWiseStatus function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see moveHomRowWiseStatus + */ +void moveHomRowWiseStatusHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the setSpeed function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see setSpeed + */ +void setSpeedHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the setAccel function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see setAccel + */ +void setAccelHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the startPotFieldMode function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see startPotFieldMode + */ +void startPotFieldModeHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the stopPotFieldMode function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see stopPotFieldMode + */ +void stopPotFieldModeHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the quit function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see quit + */ +void quitHelp(SocketObject& client , std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the quit function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see quit + */ +void setTrajectoryTypeHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the getTrajectoryType function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see getTrajectoryType + */ +void getTrajectoryTypeHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +} + +/** + * This function returns the help information text to the client + * for the isKukaLwr function. + * + * @param client connection to client which will receive the response + * @param arg unused but has to be there for matching the signature + * + * @see isKukaLwr + */ +void isKukaLwrHelp(SocketObject& client, std::string& arg) +{ + // unused + (void) arg; + + // set up the string buffers + std::string out = ""; + + // send message to client + client.Send(out); +}