diff --git a/lwrserv/include/BangBangTrajectory.h b/lwrserv/include/BangBangTrajectory.h index dcec2b5..64d1f8e 100644 --- a/lwrserv/include/BangBangTrajectory.h +++ b/lwrserv/include/BangBangTrajectory.h @@ -1,12 +1,6 @@ -#ifndef _BANGBANGTRAJCETORY_H_ -#define _BANGBANGTRAJCETORY_H_ -#include "Trajectroy.h" -#include "Vec.h" - /** * @addtogroup trajectory * @{ - * * @addtogroup BangBang * @{ * @author Philipp Schoenberger @@ -34,6 +28,10 @@ * The slowest joint is defining the speed of the other joints. * By that all joints start and stop the movement synchronously */ +#ifndef _BANGBANGTRAJCETORY_H_ +#define _BANGBANGTRAJCETORY_H_ +#include "Trajectroy.h" +#include "Vec.h" /** * Class for a Bang Bang Trajectory based on joint angles. @@ -50,7 +48,7 @@ class BangBangJointTrajectory : public Trajectory public: /** * standard constructor which will setup the class for a - * trajectory from start to end position with a synchron movement + * trajectory from start to end position with a synchron movement based on joints * * @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 ] diff --git a/lwrserv/include/FivePolyTrajectory.h b/lwrserv/include/FivePolyTrajectory.h index c87244e..e69de29 100644 --- a/lwrserv/include/FivePolyTrajectory.h +++ b/lwrserv/include/FivePolyTrajectory.h @@ -1,14 +0,0 @@ -#ifndef _FIVEPOLYTRAJECTORY_H_ -#define _FIVEPOLYTRAJECTORY_H_ -#include "Trajectroy.h" - -template -class FivePolyJointTrajectory: public Trajectory -{ -}; -template -class FivePolyCartTrajectory: public Trajectory -{ -}; - -#endif diff --git a/lwrserv/include/LSPBTrajectory.h b/lwrserv/include/LSPBTrajectory.h index 87518c1..19a8b8f 100644 --- a/lwrserv/include/LSPBTrajectory.h +++ b/lwrserv/include/LSPBTrajectory.h @@ -4,10 +4,63 @@ #include "Trajectroy.h" #include "sgn.h" +/** + * @addtogroup trajectory + * @{ + * @addtogroup LSPB + * @{ + * @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 LSPB trajectory. + * The LSPB trajectory is a trajectory with linear acceleration + * phase followed by a constant speed and a de-acceleration phase + * The speed curve looks with this like a symmetric Trapez. + * + * The slowest joint is defining the speed of the other joints. + * By that all joints start and stop the movement synchronously + */ + +/** + * 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 + */ template class LSPBJointTrajectory: public Trajectory { public: + /** + * standard constructor which will setup the class for a + * trajectory from start to end position with a synchron movement based on joints + * + * The trajectory will be of type LSPB (Linear Segment with Parabolic Blend) + * + * @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 ] + */ LSPBJointTrajectory( float sampleTimeMs, Vec maxJointVelocity, @@ -36,8 +89,6 @@ public: Vec maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity); maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity); - - // v ^ // | // v_bang| /\ <- bang bang trajectory @@ -69,60 +120,97 @@ public: Vec accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime; Vec timeMaxAcceleration = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime; Vec timeMaxVelocity = ((jointMovementAbs/2.0f).celldivide(maxJointVelocity) * 2.0f)*2.0f / sampleTime; - Vec timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity); - std::cout << timeBangBang << "timeBangBang\n"; + // calculate the time and steps of the bangbang Trajectory + Vec timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity); float steps_bangbang = timeBangBang.ceil().max(); - // calculate maxAcceleration for bangbang + // calculate maxAcceleration for a bangbang trajectory Vec currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(steps_bangbang).celldivide(steps_bangbang)*2.0f; - std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n"; + // use only a percentage of the bangbang max velocity float percentLSPB = 0.7f; Vec timeLSPB = timeBangBang * (1 + 0.25f * (1.0f - percentLSPB)); float timeBlend = ((timeBangBang /2.0f) * percentLSPB).ceil().max(); - std::cout << timeBlend << "timeBlend \n"; - - std::cout << timeLSPB << "max time \n"; + + // round up and use the maximum of all possible steps Vec minStepsPerJoint = timeLSPB.ceil(); - this->steps = minStepsPerJoint.max(); if (this->steps == 0) this->steps +=1; + // allocate an trajectory with this size this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); Vec jointLast = jointStart; Vec velocityLast(0.0f); + // initialize the accelerated counts to zero int count = 0; + for( int i = 0 ; i < this->steps; ++i) { if (i < timeBlend) { + // acceleration phase count += 1; this->nodes[i].acceleration = currMaxAcceleration; }else if (i < this->steps - timeBlend ) { + // constant speed phase this->nodes[i].acceleration = Vec(0.0f); }else { + // deacceleration phase count -= 1; this->nodes[i].acceleration = currMaxAcceleration* -1.0f; } + + // calculate velocity and joint position accordingly to the acceleration + // @warning to not add floats over and over again or the error will be huge 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) + { + // mark the current index as the last one within this trajectory + this->steps = i+1; + // set the end position + this->nodes[i].jointPos = jointEnd; + break; + } + jointLast = this->nodes[i].jointPos; velocityLast = this->nodes[i].velocity; } } }; +/** + * Class for a LSPB Trajectory based on homogenous + * Cartesian Positions. + * The joint start and stop synchron + * + * This class is based on the Trajectory joint Template so it + * all possible joint counts + * + * @TODO implement the Cartesian movement constructor for LSPB trajectories + * @see Trajectory + */ template class LSPBCartTrajectory: public Trajectory { }; +/** + * @} + * @} + */ #endif diff --git a/lwrserv/include/LinearTrajectory.h b/lwrserv/include/LinearTrajectory.h index b608258..de00740 100644 --- a/lwrserv/include/LinearTrajectory.h +++ b/lwrserv/include/LinearTrajectory.h @@ -1,16 +1,69 @@ #ifndef _LINEAR_TRAJECTORY_H_ #define _LINEAR_TRAJECTORY_H_ +/** + * @addtogroup trajectory + * @{ + * @addtogroup linear + * @{ + * @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 linear trajectory + * The linear trajectory only uses the max speed and ignores + * the acceleration parameter. + * The movement is a strict hard acceleration and deacceration. + * + * The slowest joint is defining the speed of the other joints. + * By that all joints start and stop the movement synchronously + */ #include "Trajectroy.h" #include "stdlib.h" #include "Vec.h" #include "Mat.h" + +/** + * 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 + */ template class LinearJointTrajectory : public Trajectory { public: + /** + * standard constructor which will setup the class for a + * trajectory from start to end position with a synchron movement based on joints + * + * The trajectory will be of type linear (linear constant speed) + * + * @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) ] (unused) + * @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 ] + */ LinearJointTrajectory( float sampleTimeMs, Vec maxJointVelocity, @@ -19,12 +72,10 @@ class LinearJointTrajectory : public Trajectory Vec jointEnd ) { - std::cout << "linear \n " ; float MStoSec = 1000.0f; // calculate maximum velocity and acceleration Vec maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec); Vec maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec); - std::cout << "max : " << maxJointLocalVelocity << "\n"; // calculate delta movement Vec jointMovement = jointEnd - jointStart; @@ -34,15 +85,14 @@ class LinearJointTrajectory : public Trajectory // calculate number of movement steps Vec minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity); - std::cout << "minsteps : " << minStepsPerJoint << "\n"; - std::cout << "steps : " << minStepsPerJoint.max() << "\n"; this->steps = ceil(minStepsPerJoint.max()); - std::cout << "steps : " << this->steps << "\n"; if (this->steps == 0) this->steps +=1; + // allocate an array for the joint movements this->nodes = (struct Trajectory::trajectoryNode* ) calloc(sizeof(struct Trajectory::trajectoryNode),this->steps); + // initialize the array of movements Vec jointLast = jointStart; for( int i = 0 ; i < this->steps; ++i) { @@ -51,19 +101,32 @@ class LinearJointTrajectory : public Trajectory this->nodes[i].velocity = maxJointLocalVelocity; this->nodes[i].acceleration = Vec(0.0f); } - // last step myth be to wide so cut it to the wanted position + // last step might be to wide so cut it to the wanted position this->nodes[this->steps-1].jointPos = jointEnd; - this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth + this->nodes[this->steps-1].velocity = maxJointLocalVelocity; this->nodes[this->steps-1].acceleration = Vec(0.0f); } - private: - - protected: }; +/** + * Class for a linear Trajectory based on homogenous + * Cartesian Positions. + * The joint start and stop synchron + * + * This class is based on the Trajectory joint Template so it + * all possible joint counts + * + * @TODO implement the Cartesian movement constructor for linear Trajectories + * @see Trajectory + */ template class LinearCartTrajectory : public Trajectory { }; + +/** + * @} + * @} + */ #endif diff --git a/lwrserv/include/Mat.h b/lwrserv/include/Mat.h index bd0b345..75a8c39 100644 --- a/lwrserv/include/Mat.h +++ b/lwrserv/include/Mat.h @@ -3,6 +3,35 @@ #include #include #include "Vec.h" +/** + * @addtogroup math + * @{ + * @author Philipp Schoenberger + * @version 2.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 linear trajectory + * The linear trajectory only uses the max speed and ignores + * the acceleration parameter. + * The movement is a strict hard acceleration and deacceration. + * + * The slowest joint is defining the speed of the other joints. + * By that all joints start and stop the movement synchronously + */ template class Vec; @@ -20,107 +49,167 @@ typedef Mat MatCarthesian; // template square matrix class for SIMPLE data types -template class Mat +template +class Mat { public: + /** + * standard constructor which is initialising all vales to zero + */ Mat () { - for (unsigned int j=0; j () { // nothing to do here ... } - Mat (const T aatData[SIZE][SIZE]) + /** + * copy constructor for flat arrays of the simple type + * + * @param srcData source data for the new matrix + */ + Mat (const T srcData[SIZE][SIZE]) { - for (unsigned int j=0; j (const T init_val,const T eye_val) + + /** + * constructor for an identity matrix from left top to right bottom + * + * @param initVal the value in each field beside the diagonal entries + * @param eyeVal the value on each filed exactly on the diagonal + */ + Mat (const T initVal,const T eyeVal) { - for (unsigned int j=0; j (const T init_val) + + /** + * constructor for an matrix with a defined value in each cell + * + * @param initVal the value for each field + */ + Mat (const T initVal) { - for (unsigned int j=0; j (const Mat &mat) + /** + * copy constructor for matrix-matrix copy + * + * @param srcMat the value for each field + */ + Mat (const Mat &srcMat) { - for (unsigned int j=0; j=SIZE) - i = SIZE-1; - if (j>=SIZE) - j = SIZE-1; - return m_aatData[i][j]; + if (y>=SIZE) + y = SIZE-1; + if (x>=SIZE) + x = SIZE-1; + return m_aatData[x][y]; } - T operator () (unsigned i, unsigned j) const + /** + * operator for cell access without writing to it + * + * @param x x axis position within the matrix + * @param y y axis position within the matrix + */ + T operator () (unsigned x, unsigned y) const { - if (i>=SIZE) - i = SIZE-1; - if (j>=SIZE) - j = SIZE-1; - return m_aatData[i][j]; + if (x>=SIZE) + x = SIZE-1; + if (y>=SIZE) + y = SIZE-1; + return m_aatData[x][y]; } + /** + * operator to add a simple type to each entry + * within the array + * + * @param scalar the value to add + */ Mat operator + (const T &scalar) { Mat buf; - for (unsigned int i=0; i operator - (const T &scalar) { Mat buf; - for (unsigned int i=0; i operator * (const T &scalar) { Mat buf; @@ -303,4 +392,8 @@ static Mat getRotationMatrix(float x_angle,float y_angle, float z_angl tempz=temp_z; return tempz*tempy*tempx; } +/** + * @} + * @} + */ #endif diff --git a/lwrserv/include/Robot.h b/lwrserv/include/Robot.h index a8e610e..3971745 100644 --- a/lwrserv/include/Robot.h +++ b/lwrserv/include/Robot.h @@ -13,33 +13,128 @@ typedef Vec VecTorque; class Robot { public: + /// number of joints for the default robot static const unsigned int joints = LBR_MNJ; + unsigned int joints; + /// typdef for a vector witch can hold a value for each joint typedef Vec VecJoint; - static int getJointCount(void) + protected: + VecJoint jointRangeMin; + VecJoint jointRangeMax; + VecJoint jointVelocity; + VecJoint jointAcceleration; + VecJoint jointTorque; + + public: + + /** + * default constructor for initialising the robot constraints + */ + Robot() + { + this->joints = LBR_MNJ; + this->jointRangeMin = VecJoint(-180.0f); + this->jointRangeMax = VecJoint(+180.0f); + this->jointVelocity = VecJoint(+100.0f); + this->jointAcceleration = VecJoint(+2.0f); + this->jointTorque = VecJoint(10.0f); + } + + /** + * Returns the joint count of the robot. + * + * @retval joint count + */ + int getJointCount(void) + { + return joints; + } + + /** + * Returns the minimum range for each joint of the robot + * + * @retval vector containing minimum joint angles [ degree ] + */ + VecJoint getJointRangeMin(void) + { + return VecJoint(this->jointRangeMin); + } + + /** + * Returns the maximum range for each joint of the robot + * + * @retval vector containing maximum joint angles [ degree ] + */ + VecJoint getJointRangeMax(void) + { + return VecJoint(this->jointRangeMax); + } + + /** + * Checks if the angles provided by the function parameter + * does match the valid range of the robot. + * + * @retval true if all angles are matching the range or false otherwise + */ + bool validateJointRange(VecJoint jointAngles) { + for(unsigned int i = 0 ; i < joints ; ++i) + { + // angle to big + if (jointAngles(i) > this->jointRangeMax(i)) + return false; + + // angle to small + if (jointAngles(i) < this->jointRangeMin(i)) + return false; + } + return true; } - static VecJoint getJointRange(void) + + /** + * Returns the maximum velocity for each joint of the robot + * + * @retval vector containing maximum velocity for each joint [ degree/seconds ] + */ + VecJoint getJointVelocity(void) { - return VecJoint(180.0f); + return VecJoint(this->jointVelocity); } - static VecJoint getJointVelocity(void) + + /** + * Returns the maximum acceleration for each joint of the robot + * + * @retval vector containing maximum acceleration for each joint [ degree/seconds^2 ] + */ + VecJoint getJointAcceleration(void) { - return VecJoint(200.0f); + return VecJoint(this->jointAcceleration); } - static VecJoint getJointAcceleration(void) + + /** + * Returns the maximum torque for each joint of the robot + * + * @retval vector containing maximum torque for each joint [ Nm ] + */ + VecJoint getJointTorqueMax(void) { - return VecJoint(10.0f); + return VecJoint(this->jointTorque); } - static Vec getDhParameter(unsigned int joint) + /** + * Returns the david hadenberg parameter for the requested joint + * + * @retval vector containing the 4 parameter + */ + Vec getDhParameter(unsigned int joint) { // unused parameter (void) joint; return Vec(0.0f); } - + struct RobotConfig { bool elbow; @@ -47,8 +142,38 @@ class Robot bool j1os; }; - static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config ); - static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); + /** + * Calculates the joint angles for a given Position and arm configuration + * + * @param M homogenious cartesian position + * @param config the robot arm configuration ( elbow etc.) + * @return the joint angles for the requested position + */ + VecJoint backwardCalc(MatCarthesian M, struct RobotConfig config ) + { + // unused + (void) M; + (void) config; + + return VecJoint(0.0f); + } + + /** + * Calculates the homogenious cartesian position for a given angle + * and robot arm configuration + * + * @param M homogenious cartesian position + * @param config the robot arm configuration ( elbow etc.) + * @return the joint angles for the requested position + */ + MatCarthesian forwardCalc(VecJoint j, struct RobotConfig config ) + { + // unused + (void) j; + (void) config; + + return MatCarthesian(0.0f,1.0f); + } }; #endif diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index 74d99c7..9a95e35 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -30,12 +30,7 @@ private: } commanded; struct { - struct { - Robot::VecJoint velocity; - Robot::VecJoint accelaration; - Robot::VecJoint torque; - Robot::VecJoint range; - } max; + Robot* robotClass; float currentVelocityPercentage; float currentAccelerationPercentage; } robot; @@ -60,7 +55,7 @@ private: static SvrData* single; - void updateMessurementData( + void updateMeasurementData( Robot::VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque @@ -72,10 +67,7 @@ public: friRemote* getFriRemote(); - void updateMessurement(); - - void lock(); - void unlock(); + void updateMeasurement(); Robot::VecJoint getMeasuredJointPos(); MatCarthesian getMeasuredCartPos(); diff --git a/lwrserv/include/commandsHelp.h b/lwrserv/include/commandsHelp.h index a26bcd0..a8e2774 100644 --- a/lwrserv/include/commandsHelp.h +++ b/lwrserv/include/commandsHelp.h @@ -33,7 +33,7 @@ void stopPotFieldModeHelp (SocketObject& client); void setTrajectoryTypeHelp (SocketObject& client); // get the current trajectory type void getTrajectoryTypeHelp (SocketObject& client); -//Quit +// Quit void quitHelp (SocketObject& client); // check if we use a kuka void isKukaLwrHelp (SocketObject& client); diff --git a/lwrserv/include/friComm.h b/lwrserv/include/friComm.h index fab420b..1e88979 100644 --- a/lwrserv/include/friComm.h +++ b/lwrserv/include/friComm.h @@ -23,7 +23,7 @@ **************************************************************************** friComm.h - NOTE: FRI (Fast Research inteface) is subject to radical change + NOTE: FRI (Fast Research interface) is subject to radical change [FH]}} @@ -32,7 +32,7 @@ \author (Andreas Stemmer,DLR) \author (Guenter Schreiber,KUKA) \file - \brief This header file describes the basic communcation structures for + \brief This header file describes the basic communication structures for the FastRobotInterface diff --git a/lwrserv/include/lwr4.h b/lwrserv/include/lwr4.h index 9e263b9..4cb67fe 100644 --- a/lwrserv/include/lwr4.h +++ b/lwrserv/include/lwr4.h @@ -5,20 +5,79 @@ #include "friComm.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 { - public : - static Robot::VecJoint getJointRange(void); - static Robot::VecJoint getJointVelocity(void); - static Robot::VecJoint getJointAcceleration(void); - - static Vec getDhParameter(unsigned int joint); - - static Robot::VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config ); - static MatCarthesian forwardCalc(Robot::VecJoint, struct RobotConfig config ); + // 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); diff --git a/lwrserv/src/SvrData.cpp b/lwrserv/src/SvrData.cpp index 74084f8..174edab 100644 --- a/lwrserv/src/SvrData.cpp +++ b/lwrserv/src/SvrData.cpp @@ -11,54 +11,83 @@ #include "Mat.h" #include "Vec.h" #include "Robot.h" +#include "lwr4.h" bool SvrData::instanceFlag = false; SvrData* SvrData::single = 0; +/** + * private Default Constructor which is initializing everything to the default parameters. + * + * By default 10% of the Maximum Velocity and Acceleration is used. + * Also the default robot is the LWR4 and a sample time of 5 ms + * + * This is a private constructor so only getInstance can call this + * Constructor once and keep this class a singleton. + * + * @see getInstance + */ SvrData::SvrData(void) { - // these are the default values for the robot - double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0}; - double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0}; - double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0}; - double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0}; - + // only use 10 percent of the maximum velocity by default robot.currentVelocityPercentage = 10; robot.currentAccelerationPercentage = 10; - for (unsigned int i = 0; i < Robot::joints; ++i) - { - robot.max.velocity(i) = maxVelJnt[i]; - robot.max.accelaration(i) = maxAccJnt[i]; - robot.max.torque(i) = maxTrqJnt[i]; - robot.max.range(i) = maxRangeJnt[i]; - } + // use the lwr4 robot by default + robot.robotClass = new LWR4(); + // use 5ms interval for each step trajectory.sampleTimeMs = 5.0f; + + // do not cancel current trajectory (there also should not be any at this point) trajectory.cancel = false; // init mutex for database if (pthread_mutex_init(&dataLock, NULL) != 0) - printf("mutex init failed\n"); + printf("mutex initializing failed\n"); + // connect to the fri remote interface this->friInst = new friRemote(); } +/** + * Destructor for cleaning up the memory + */ SvrData::~SvrData() { pthread_mutex_destroy(&dataLock); + delete robot.robotClass; } +/** + * Returns always the same reference to the SvrData. + * For the first call it will call the private Constructor otherwise + * it will return the same reference like the first time. + * + * @return A pointer to the SvrData singleton class + */ + SvrData* SvrData::getInstance () { + // check if the instance was already initialized if (instanceFlag) return single; + // first time so we have to create a new instance of SvrData single = new SvrData(); + + // mark the success on initializing to not allocate a second + // SvrData reference instanceFlag = true; + return single; } +/** + * Returns the same reference to the FRI remote interface + * + * @return a pointer to the FRI class + */ friRemote* SvrData::getFriRemote() { friRemote* retval = NULL; @@ -66,84 +95,142 @@ friRemote* SvrData::getFriRemote() pthread_mutex_lock(&dataLock); retval = this->friInst; pthread_mutex_unlock(&dataLock); + return retval; } -void SvrData::updateMessurementData( +/** + * This function updates the measured Units directly + * via function parameters + * + * @param newJointPos new measured Unit for the joint angles + * @param newCartPos new measured homogeneous cartesian positions + * @param newForceAndTorque new measured force and torque for the end effector + * + * @see updateMeasurement + */ +void SvrData::updateMeasurementData( Robot::VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque ) { pthread_mutex_lock(&dataLock); + this->messured.jointPos = newJointPos; this->messured.cartPos = newCartPos; this->messured.forceAndTorque = newForceAndTorque; + pthread_mutex_unlock(&dataLock); } -void SvrData::updateMessurement() +/** + * This function updates the measured Units directly + * via the FRI remote instance and saves them into the database. + * + * @see updateMeasurementData + */ +void SvrData::updateMeasurement() { Robot::VecJoint newJointPos(0.0f); MatCarthesian newCartPos(0.0f,1.0f); VecTorque newForceAndTorque(0.0f); - /// do the update using fri + /// do the update using the FRI Interface /// update current joint positions - friInst->getState(); - FRI_STATE state = friInst->getState(); Robot::VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); Robot::VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); newJointPos = newJointPosComanded + newJointPosOffset; + + // convert the joints from radian to degree newJointPos = newJointPos * ( 180.0f / M_PI); /// update current cart position newCartPos = friInst->getMsrCartPosition(); - + /// update current force and torque newForceAndTorque = friInst->getFTTcp(); - std::cout << newJointPos << " current newJointPos \n" << state << "\n"; - - updateMessurementData(newJointPos,newCartPos,newForceAndTorque); -} - -void SvrData::lock() -{ - pthread_mutex_lock(&dataLock); -} - -void SvrData::unlock() -{ - pthread_mutex_unlock(&dataLock); + /// save the new values to the database + updateMeasurementData(newJointPos,newCartPos,newForceAndTorque); } -Robot::VecJoint SvrData::getMessuredJointPos() +/** + * This function returns the current measurement of the joints + * within the database. + * This value will be updated if updateMeasurement was called + * which is done by the robot server thread. + * + * @see updateMeasurement + * @see threadRobotMovement + */ +Robot::VecJoint SvrData::getMeasuredJointPos() { Robot::VecJoint buf; + pthread_mutex_lock(&dataLock); buf = messured.jointPos; pthread_mutex_unlock(&dataLock); + return buf; } +/** + * This function returns the current measurement homogeneous + * cartesian positions within the database. + * + * This value will be updated if updateMeasurement was called + * which is done by the robot server thread. + * + * @see updateMeasurement + * @see threadRobotMovement + */ MatCarthesian SvrData::getMessuredCartPos() { MatCarthesian buf; + pthread_mutex_lock(&dataLock); buf = messured.cartPos; pthread_mutex_unlock(&dataLock); + return buf; } + +/** + * This function returns the current measurement force and + * torque within the database. + * + * This value will be updated if updateMeasurement was called + * which is done by the robot server thread. + * + * @see updateMeasurement + * @see threadRobotMovement + */ VecTorque SvrData::getMessuredForceTorque() { VecTorque buf; + pthread_mutex_lock(&dataLock); buf = messured.forceAndTorque; pthread_mutex_unlock(&dataLock); + return buf; } +/** + * This function returns the current Jacobian matrix. + * + * This value will be updated if updateMeasurement was called + * which is done by the robot server thread. + * + * @param data pointer for the matrix for the jacobian matrix + * @param size the size of the given data pointer to prevent segmentation faults + * + * @return 0 on success and the negative error code if an error occurred. + * + * @see updateMeasurement + * @see threadRobotMovement + */ int SvrData::getMessuredJacobian(float* data, size_t size) { if (data == NULL) @@ -154,31 +241,67 @@ int SvrData::getMessuredJacobian(float* data, size_t size) pthread_mutex_lock(&dataLock); memcpy(data,messured.jacobian,size); pthread_mutex_unlock(&dataLock); + return 0; } +/** + * This function returns the current commanded joint positions + * + * @return Vector for the current commanded joint values in unit [degree] + * + * @see setCommandedJointPos + */ Robot::VecJoint SvrData::getCommandedJointPos() { Robot::VecJoint buff; + pthread_mutex_lock(&dataLock); buff = commanded.jointPos; pthread_mutex_unlock(&dataLock); + return buff; } + +/** + * This function returns the current commanded + * + * @return Vector for the current commanded joint values in unit [degree] + * + * @see getCommandedJointPos + */ void SvrData::setCommandedJointPos(Robot::VecJoint newJointPos) { pthread_mutex_lock(&dataLock); commanded.jointPos = newJointPos; pthread_mutex_unlock(&dataLock); } + +/** + * This function returns the current commanded position + * + * @return Matrix for the current commanded cartesian homogeneous values + * + * @see setCommandedCartPos + */ MatCarthesian SvrData::getCommandedCartPos () { - MatCarthesian buff; + MatCarthesian buff; + pthread_mutex_lock(&dataLock); buff = commanded.cartPos; pthread_mutex_unlock(&dataLock); + return buff; } + +/** + * This function sets the current commanded position + * + * @return Matrix for the current commanded cartesian homogeneous values + * + * @see getCommandedCartPos + */ void SvrData::setCommandedCartPos(MatCarthesian newCartPos) { pthread_mutex_lock(&dataLock); @@ -187,40 +310,96 @@ void SvrData::setCommandedCartPos(MatCarthesian newCartPos) } +/** + * This function returns the current maximum force and torque per joint + * for the configured Robot. + * + * @return Vector containing the Torque in the unit [ Nm ] + */ Robot::VecJoint SvrData::getMaxTorque() { - Robot::VecJoint buff; + Robot::VecJoint retval; + pthread_mutex_lock(&dataLock); - buff = robot.max.torque; + retval = robot.robotClass->getJointCount(); pthread_mutex_unlock(&dataLock); - return buff; + return retval; } +/** + * This function returns the current maximum acceleration per joint + * for the configured Robot. + * + * @return Vector containing the maximum acceleration [ degree/ seconds^2 ] + */ Robot::VecJoint SvrData::getMaxAcceleration() { Robot::VecJoint retval(0.0f); + pthread_mutex_lock(&dataLock); - retval = robot.max.accelaration; + retval = robot.robotClass->getJointAcceleration(); pthread_mutex_unlock(&dataLock); + return retval; } +/** + * This function returns the current maximum velocity per joint + * for the configured Robot. + * + * @return Vector containing the maximum velocity [ degree/ seconds ] + */ Robot::VecJoint SvrData::getMaxVelocity() { Robot::VecJoint retval(0.0f); + pthread_mutex_lock(&dataLock); - retval = robot.max.velocity; + retval = robot.robotClass->getJointVelocity(); pthread_mutex_unlock(&dataLock); + return retval; } + +/** + * This function returns the current maximum range per joint + * for the configured Robot. + * + * @return Vector containing the maximum angle for the joints [ degree ] + */ Robot::VecJoint SvrData::getMaxRange() { Robot::VecJoint retval(0.0f); + pthread_mutex_lock(&dataLock); - retval = robot.max.range; + retval = robot.robotClass->getJointRangeMax(); pthread_mutex_unlock(&dataLock); + return retval; } + +/** + * This function returns the current minimum range per joint + * for the configured Robot. + * + * @return Vector containing the minimum angle for the joints [ degree ] + */ +Robot::VecJoint SvrData::getMinRange() +{ + Robot::VecJoint retval(0.0f); + + pthread_mutex_lock(&dataLock); + retval = robot.robotClass->getJointRangeMin(); + pthread_mutex_unlock(&dataLock); + + return retval; +} + +/** + * This function returns the current minimum range per joint + * for the configured Robot. + * + * @return Vector containing the minimum angle for the joints [ degree ] + */ bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck) { // save temporal the max range for short time lock @@ -230,7 +409,7 @@ bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck) pthread_mutex_unlock(&dataLock); // check if the value is in plus or minus range - for (int i = 0; ijoints; i++) { if (abs(vectorToCheck(i)) > abs(maxJointRange(i))) { @@ -264,7 +443,7 @@ Robot::VecJoint SvrData::getRobotVelocity() pthread_mutex_lock(&dataLock); percent = robot.currentVelocityPercentage / 100.0f; - retval = robot.max.velocity * percent; + retval = robot.robotClass->getJointVelocity() * percent; pthread_mutex_unlock(&dataLock); return retval; } @@ -275,6 +454,7 @@ float SvrData::getRobotVelocityPercentage() pthread_mutex_lock(&dataLock); retval = robot.currentVelocityPercentage; pthread_mutex_unlock(&dataLock); + return retval; } int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage) @@ -290,6 +470,7 @@ int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage) pthread_mutex_lock(&dataLock); robot.currentAccelerationPercentage = newAccelerationPercentage; pthread_mutex_unlock(&dataLock); + return retval; } Robot::VecJoint SvrData::getRobotAcceleration() @@ -299,8 +480,9 @@ Robot::VecJoint SvrData::getRobotAcceleration() pthread_mutex_lock(&dataLock); percent = robot.currentAccelerationPercentage / 100.0f; - retval = robot.max.accelaration * percent; + retval = robot.robotClass->getJointAcceleration() * percent; pthread_mutex_unlock(&dataLock); + return retval; } float SvrData::getRobotAccelerationPercentage() @@ -310,6 +492,7 @@ float SvrData::getRobotAccelerationPercentage() pthread_mutex_lock(&dataLock); retval = robot.currentAccelerationPercentage; pthread_mutex_unlock(&dataLock); + return retval; } unsigned int SvrData::getJoints() @@ -319,6 +502,7 @@ unsigned int SvrData::getJoints() pthread_mutex_lock(&dataLock); retval = Robot::joints; pthread_mutex_unlock(&dataLock); + return retval; } diff --git a/lwrserv/src/SvrHandling.cpp b/lwrserv/src/SvrHandling.cpp index 6792169..9cefe7d 100644 --- a/lwrserv/src/SvrHandling.cpp +++ b/lwrserv/src/SvrHandling.cpp @@ -21,6 +21,7 @@ */ void printUsage(SocketObject& client, std::string& cmd) { + // search if the cmd command was specified for (unsigned int i = 0 ; i < commandCount; ++i) { if (cmd == commands[i].longVersion || cmd == commands[i].aberration) @@ -38,7 +39,7 @@ void printUsage(SocketObject& client, std::string& cmd) // no command was specified. // print only short versions of the commands - client.Send("List of Commands with aberration and long form\n For detailed help try \"help \"\n"); + client.Send("List of Commands with aberration and long form\nFor detailed help try \"help \"\n"); client.Send("short version \t-\t long version\n"); for (unsigned int i = 0 ; i < commandCount; ++i) { @@ -50,7 +51,10 @@ void printUsage(SocketObject& client, std::string& cmd) /** * Normal Constructor - * Initalizes the command array with long/short string version and function pointers for each command. + * Initalizes the command array with long/short string version + * for the commands. + * Also sets function pointers for processing + * and printing the detailed help information for each command */ SvrHandling::SvrHandling() { @@ -148,27 +152,36 @@ void * SvrHandling::threadRobotMovement(void *arg) // get current robot position int tries = 2; + // try to connect to the robot + // there is a bug in the FRI so we have to try it multiple times for (int i= 0 ; i < tries ; ++i) { + // update the measured Data if (REAL_ROBOT) - SvrData::getInstance()->updateMessurement(); + SvrData::getInstance()->updateMeasurement(); + + // query the current position of the robot currentJointPos = SvrData::getInstance()->getMeasuredJointPos(); - SvrData::getInstance()->setCommandedJointPos(currentJointPos); + // move robot to current measured position to stop any kind of movement float newJointPosToSend[Robot::joints] = {0.0f}; currentJointPos.getData(newJointPosToSend); if (REAL_ROBOT) friInst->doPositionControl(newJointPosToSend); } - std::cout << "init position is " << currentJointPos << "\n"; + std::cout << "starting position is " << currentJointPos << "\n"; + + // start the client command processing loop while(true) { + // update all measurements in each tic if (REAL_ROBOT) - SvrData::getInstance()->updateMessurement(); + SvrData::getInstance()->updateMeasurement(); bool positionChanged = false; + // check if we have to cancel current trajectory bool cancel = SvrData::getInstance()->getTrajectoryCancel(); if (cancel) @@ -184,13 +197,13 @@ void * SvrHandling::threadRobotMovement(void *arg) goto end; } - // check for new trajectory + // check if we currently have a trajectory if (currentTrajectory == NULL) { - // get next trajectory from svrData + // try to get next trajectory from svrData currentTrajectory = SvrData::getInstance()->popTrajectory(); - // no new trajectory + // check if it was a valid trajectory if(currentTrajectory == NULL) { // mark that we reached the end of the trajectory @@ -199,7 +212,7 @@ void * SvrHandling::threadRobotMovement(void *arg) } else { - std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<"\n"; + std::cout <<"new Trajectory with " << currentTrajectory->getSteps()<<" steps\n"; } } @@ -210,6 +223,7 @@ void * SvrHandling::threadRobotMovement(void *arg) { case MovementCartBased: { + // use new carthesian position coming from the trajectory currentCartPos = currentTrajectory->getNextCartPos(); positionChanged = true; currentMovementType = MovementCartBased; @@ -217,6 +231,7 @@ void * SvrHandling::threadRobotMovement(void *arg) break; case MovementJointBased: { + // use new joint angles coming from the trajectory currentJointPos = currentTrajectory->getNextJointPos(); positionChanged = true; currentMovementType = MovementJointBased; @@ -224,7 +239,7 @@ void * SvrHandling::threadRobotMovement(void *arg) break; default: { - // invalid trajectory skip it + // invalid trajectory delete and skip it delete currentTrajectory; currentTrajectory = NULL; goto end; @@ -234,43 +249,64 @@ void * SvrHandling::threadRobotMovement(void *arg) } end: + // actually do the movement to the new position or to the + // current one if there was no new position. + // This has to be send to robot to not decrease quality of + // the signal switch (currentMovementType) { case MovementJointBased: { - // send the new joint values + // convert joint values to float array float newJointPosToSend[Robot::joints] = {0.0f}; Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); newJointPosToSendRad.getData(newJointPosToSend); + + // send the new joint values to the robot if (REAL_ROBOT) friInst->doPositionControl(newJointPosToSend); } break; case MovementCartBased: { + // use default stiffness and damping float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0}; float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7}; + + // convert cartesian position to an array of floats float newCartPosToSend[12] = {0.0f}; currentCartPos.getData(newCartPosToSend); + + // send the new cartesian values to the robot if (REAL_ROBOT) friInst->doCartesianImpedanceControl(newCartPosToSend,Stiff, Damp, NULL, NULL, true); } break; } - // start timer for the last sent msg - // TODO - - // check if current trajectory is over + // start timer for the last sent message + // TODO measure the time for updating the Position, so do not update the + // positions faster than needed ( FRI Remote does actually not wait for sample time like suggested before) + + // check if current trajectory has reached the last step if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0) { std::cout << " Trajectory finished \n "; + // each trajectory gets a new number starting from 1 static int trajcetorycount = 0; trajcetorycount +=1; - currentTrajectory->saveJointToFile(std::string("trajectory/a.csv")); - // invalid trajectory skip it + // assemble the filename + std::string filename = ""; + std::stringstream ss; + ss << "trajectory/trajectory_"<< trajcetorycount << ".csv"; + filename = ss.str(); + + + currentTrajectory->saveJointToFile(filename); + + // free the current trajectory delete currentTrajectory; currentTrajectory = NULL; } @@ -289,7 +325,7 @@ SvrHandling::~SvrHandling() /** * Starts the server thread with default port. * - * @info will never return and should run in a thread + * @info will never return. So it should run in a separate thread. */ void SvrHandling::run() { @@ -306,10 +342,10 @@ void SvrHandling::run(int port) { std::cout << timestamp() + "Starting " << SVR_NAME << " on port "<< port <<"\n"; - // only abourt loop if state done is reached + // only abort loop if state done is reached while (c_state != done) { - // create new socket and bind it to the speciffic port + // create new socket and bind it to the specific port SocketObject *socket = new SocketObject; if (socket->Bind(port) <0) { diff --git a/lwrserv/test/main.cpp b/lwrserv/test/main.cpp index 68e7da8..2c7e9e4 100644 --- a/lwrserv/test/main.cpp +++ b/lwrserv/test/main.cpp @@ -1,4 +1,29 @@ #ifdef UNIT_TEST +/** + * @addtogroup testsuite + * @{ + * @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 + * comming from the client interface + */ #include "CppUTest/CommandLineTestRunner.h" #include "CppUTest/TestHarness.h" @@ -6,11 +31,18 @@ #include "CppUTest/TestTestingFixture.h" #include "CppUTest/PlatformSpecificFunctions.h" - - +/** + * main function for the test suite + * + * only start the cpputest + */ int main(int ac, char** av) { return CommandLineTestRunner::RunAllTests(ac, av); } +/** + * @} + */ + #endif diff --git a/lwrserv/test/test_mat.cpp b/lwrserv/test/test_mat.cpp index b102b85..f797057 100644 --- a/lwrserv/test/test_mat.cpp +++ b/lwrserv/test/test_mat.cpp @@ -1,3 +1,33 @@ +/** + * @addtogroup testsuite + * @{ + * @addtogroup matrixtests + * @{ + * @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 + */ #include "CppUTest/TestHarness.h" #include "CppUTest/TestRegistry.h" #include "CppUTest/TestOutput.h" @@ -10,10 +40,18 @@ int testMat [TESTSIZE][TESTSIZE]; int testVec [TESTSIZE]; float tolerance = 0.00001f; +/** + * testgroup for the matrix class + */ TEST_GROUP(Matrix) { + /** + * setup for for all test suite + * This Function is called before every Test case + */ void setup() { + // initializing the testmatrix int i = 1; int j = 1; for (int x = 0; x a; for(int x = 0 ; x < TESTSIZE ; ++x) { @@ -44,62 +91,70 @@ TEST(Matrix, vectorInvRotX) a(x,x) = val; } } - + + // set the rotation matrix to 45 degree in z a(0,0) = 1.0f; a(1,1) = cos(45.0f *M_PI/180.0f); a(1,2) = -sin(45.0f *M_PI/180.0f); a(2,2) = cos(45.0f *M_PI/180.0f); a(2,1) = sin(45.0f *M_PI/180.0f); - - float deg = 45.0f*M_PI/180.0f; + + // check the matrix after setting it up DOUBLES_EQUAL(1.0f,a(0,0), tolerance); DOUBLES_EQUAL(0.0f,a(0,1), tolerance); DOUBLES_EQUAL(0.0f,a(0,2), tolerance); DOUBLES_EQUAL(0.0f,a(0,3), tolerance); - + DOUBLES_EQUAL(0.0f,a(1,0), tolerance); DOUBLES_EQUAL(cos(deg),a(1,1), tolerance); DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance); DOUBLES_EQUAL(0.0f,a(1,3), tolerance); - + DOUBLES_EQUAL(0.0f,a(2,0), tolerance); DOUBLES_EQUAL(sin(deg),a(2,1), tolerance); DOUBLES_EQUAL(cos(deg),a(2,2), tolerance); DOUBLES_EQUAL(0.0f,a(2,3), tolerance); - + DOUBLES_EQUAL(0.0f,a(3,0), tolerance); DOUBLES_EQUAL(0.0f,a(3,1), tolerance); DOUBLES_EQUAL(0.0f,a(3,2), tolerance); DOUBLES_EQUAL(1.0f,a(3,3), tolerance); - + + // invert the matrix a = a.inv(); - + + // check for the negative angle for a z rotation deg = -45.0f*M_PI/180.0f; DOUBLES_EQUAL(1.0f,a(0,0), tolerance); DOUBLES_EQUAL(0.0f,a(0,1), tolerance); DOUBLES_EQUAL(0.0f,a(0,2), tolerance); DOUBLES_EQUAL(0.0f,a(0,3), tolerance); - + DOUBLES_EQUAL(0.0f,a(1,0), tolerance); DOUBLES_EQUAL(cos(deg),a(1,1), tolerance); DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance); DOUBLES_EQUAL(0.0f,a(1,3), tolerance); - + DOUBLES_EQUAL(0.0f,a(2,0), tolerance); DOUBLES_EQUAL(sin(deg),a(2,1), tolerance); DOUBLES_EQUAL(cos(deg),a(2,2), tolerance); DOUBLES_EQUAL(0.0f,a(2,3), tolerance); - + DOUBLES_EQUAL(0.0f,a(3,0), tolerance); DOUBLES_EQUAL(0.0f,a(3,1), tolerance); DOUBLES_EQUAL(0.0f,a(3,2), tolerance); DOUBLES_EQUAL(1.0f,a(3,3), tolerance); } +/** + * Test the matrix for invert an non identity matrix + * with a y rotation + */ TEST(Matrix, vectorInvRotY) { + // setup the test matrix to an identity matrix Mat a(0.0f); float deg = 45.0f*M_PI/180.0f; @@ -136,12 +191,17 @@ TEST(Matrix, vectorInvRotY) DOUBLES_EQUAL(1.0f,a(3,3), tolerance); } +/** + * Test the matrix for invert an identity matrix + */ TEST(Matrix, matrixInvEye) { Mat a(0.0f , 1.0f); // do the invert a = a.inv(); + + // check if the matrix is still an identity matrix DOUBLES_EQUAL(1.0f,a(0,0), tolerance); DOUBLES_EQUAL(0.0f,a(0,1), tolerance); DOUBLES_EQUAL(0.0f,a(0,2), tolerance); @@ -162,6 +222,11 @@ TEST(Matrix, matrixInvEye) DOUBLES_EQUAL(0.0f,a(3,2), tolerance); DOUBLES_EQUAL(1.0f,a(3,3), tolerance); } + + +/** + * Test if the matrix constructor for identity matrix is working + */ TEST(Matrix, matrixEye) { Mat a(0.0f , 1.0f); @@ -186,6 +251,11 @@ TEST(Matrix, matrixEye) DOUBLES_EQUAL(0.0f,a(3,2), tolerance); DOUBLES_EQUAL(1.0f,a(3,3), tolerance); } + +/** + * Test if the determinant function is working with an identity matrix + * This should always return an 1 + */ TEST(Matrix, matrixDeterminantEye) { //eye matrix @@ -193,6 +263,12 @@ TEST(Matrix, matrixDeterminantEye) DOUBLES_EQUAL(1.0f,a.determinant(),tolerance); } + +/** + * Test if the determinant function is working with an identity matrix + * with only 3 dimensions. + * This should always return an 1 + */ TEST(Matrix, matrixDeterminantEye3) { //eye matrix @@ -200,6 +276,11 @@ TEST(Matrix, matrixDeterminantEye3) DOUBLES_EQUAL(1.0f,a.determinant(),tolerance); } + +/** + * Test if the determinant function is working with an identity matrix + * with 1 dimension should always return the first cell + */ TEST(Matrix, matrixDeterminantSimple) { Mat a; @@ -207,6 +288,11 @@ TEST(Matrix, matrixDeterminantSimple) DOUBLES_EQUAL(50.0f,a.determinant(), tolerance); } + +/** + * Test if the determinant function is working with an identity matrix + * with 2 dimension should always return the multiplication of the diagonal + */ TEST(Matrix, matrixDeterminantSimple2) { Mat a; @@ -215,6 +301,11 @@ TEST(Matrix, matrixDeterminantSimple2) DOUBLES_EQUAL(50.0f,a.determinant(), tolerance); } +/** + * Test if the determinant function is working with an complete non zero matrix + * with 2 dimension should always return the multiplication of the diagonal + * and negative counter diagonal multiplication + */ TEST(Matrix, matrixDeterminantSimple3) { Mat a; @@ -225,6 +316,11 @@ TEST(Matrix, matrixDeterminantSimple3) DOUBLES_EQUAL(0.0f,a.determinant(), tolerance); } +/** + * Test if the determinant function is working with an complete non zero matrix + * with 2 dimension should always return the multiplication of the diagonal + * and negative counter diagonal multiplication + */ TEST(Matrix, matrixDeterminantSimple4) { Mat a; @@ -235,20 +331,16 @@ TEST(Matrix, matrixDeterminantSimple4) DOUBLES_EQUAL(75.0f,a.determinant(), tolerance); } +/** + * Test if the transpose function is working with an identity matrix + * with 4 dimension should always be the same again + */ TEST(Matrix, matrixTransposeEye) { - Mat a; - for(int x = 0 ; x < TESTSIZE ; ++x) - { - for (int y = 0; y a(0.0f,1.0f); + // check if the identity matrix is correctly created DOUBLES_EQUAL(1.0f,a(0,0), tolerance); DOUBLES_EQUAL(0.0f,a(0,1), tolerance); DOUBLES_EQUAL(0.0f,a(0,2), tolerance); @@ -269,9 +361,10 @@ TEST(Matrix, matrixTransposeEye) DOUBLES_EQUAL(0.0f,a(3,2), tolerance); DOUBLES_EQUAL(1.0f,a(3,3), tolerance); + // invert the identity matrix a = a.inv(); - // should stille be the same + // should still be the same DOUBLES_EQUAL(1.0f,a(0,0), tolerance); DOUBLES_EQUAL(0.0f,a(0,1), tolerance); DOUBLES_EQUAL(0.0f,a(0,2), tolerance); @@ -292,12 +385,17 @@ TEST(Matrix, matrixTransposeEye) DOUBLES_EQUAL(0.0f,a(3,2), tolerance); DOUBLES_EQUAL(1.0f,a(3,3), tolerance); } + +/** + * Test if the transpose function is working with an identity matrix + * with 4 dimension should always be the same again + */ TEST(Matrix, vectorMultiply) { Mat a = testMat; Vec v = testVec; - //Vec b = a * v; + Vec b = a * v; for (int x = 0; x