diff --git a/lwrserv/include/Robot.h b/lwrserv/include/Robot.h index 6d41c02..a8e610e 100644 --- a/lwrserv/include/Robot.h +++ b/lwrserv/include/Robot.h @@ -50,6 +50,5 @@ class Robot static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config ); static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); }; -typedef Vec VecJoint; #endif diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index 8a337d8..74d99c7 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -19,22 +19,22 @@ class SvrData: public Singleton { private: struct { - VecJoint jointPos; + Robot::VecJoint jointPos; MatCarthesian cartPos; VecTorque forceAndTorque; float jacobian[FRI_CART_VEC * Robot::joints]; } messured; struct { - VecJoint jointPos; + Robot::VecJoint jointPos; MatCarthesian cartPos; } commanded; struct { struct { - VecJoint velocity; - VecJoint accelaration; - VecJoint torque; - VecJoint range; + Robot::VecJoint velocity; + Robot::VecJoint accelaration; + Robot::VecJoint torque; + Robot::VecJoint range; } max; float currentVelocityPercentage; float currentAccelerationPercentage; @@ -61,7 +61,7 @@ private: void updateMessurementData( - VecJoint newJointPos, + Robot::VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque ); @@ -77,32 +77,32 @@ public: void lock(); void unlock(); - VecJoint getMeasuredJointPos(); + Robot::VecJoint getMeasuredJointPos(); MatCarthesian getMeasuredCartPos(); int getMessuredJacobian(float* data, size_t size); VecTorque getMessuredForceTorque(); - VecJoint getMessuredJointPos(); + Robot::VecJoint getMessuredJointPos(); MatCarthesian getMessuredCartPos(); - VecJoint getCommandedJointPos(); - void setCommandedJointPos(VecJoint newJointPos); + Robot::VecJoint getCommandedJointPos(); + void setCommandedJointPos(Robot::VecJoint newJointPos); MatCarthesian getCommandedCartPos(); void setCommandedCartPos(MatCarthesian newCartPos); - VecJoint getMaxTorque(); - VecJoint getMaxVelocity(); - VecJoint getMaxAcceleration(); - VecJoint getMaxRange(); + Robot::VecJoint getMaxTorque(); + Robot::VecJoint getMaxVelocity(); + Robot::VecJoint getMaxAcceleration(); + Robot::VecJoint getMaxRange(); - bool checkJointRange(VecJoint vectorToCheck); + bool checkJointRange(Robot::VecJoint vectorToCheck); - VecJoint getRobotVelocity(); + Robot::VecJoint getRobotVelocity(); float getRobotVelocityPercentage(); int setRobotVelocityPercentage(float newVelocityPercentage); - VecJoint getRobotAcceleration(); + Robot::VecJoint getRobotAcceleration(); float getRobotAccelerationPercentage(); int setRobotAccelerationPercentage(float newAccelerationPercentage); @@ -123,16 +123,16 @@ public: enum TrajectoryType getTrajectoryType(); void setTrajectoryType(enum TrajectoryType newType); - class Trajectory* createTrajectory(VecJoint newJointPos); + class Trajectory* createTrajectory(Robot::VecJoint newJointPos); class Trajectory* createTrajectory(MatCarthesian newJointPos); class Trajectory* calculateTrajectory( - VecJoint maxJointVelocity, - VecJoint maxJointAcceleration, - VecJoint jointStart, - VecJoint jointEnd + Robot::VecJoint maxJointVelocity, + Robot::VecJoint maxJointAcceleration, + Robot::VecJoint jointStart, + Robot::VecJoint jointEnd ); - int calculateAndAddNewTrajectory(VecJoint jointEnd); + int calculateAndAddNewTrajectory(Robot::VecJoint jointEnd); void setTrajectoryPotfieldMode(bool newstate); bool getTrajectoryPotfieldMode(); diff --git a/lwrserv/include/lwr4.h b/lwrserv/include/lwr4.h index 5a972cd..40d245a 100644 --- a/lwrserv/include/lwr4.h +++ b/lwrserv/include/lwr4.h @@ -10,18 +10,14 @@ class LWR4 : public Robot { public : - const static int joints = 7; - typedef Vec VecJoint; - - - static VecJoint getJointRange(void); - static VecJoint getJointVelocity(void); - static VecJoint getJointAcceleration(void); + static Robot::VecJoint getJointRange(void); + static Robot::VecJoint getJointVelocity(void); + static Robot::VecJoint getJointAcceleration(void); static Vec getDhParameter(unsigned int joint); - static VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config ); - static MatCarthesian forwardCalc(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/SvrData.cpp b/lwrserv/src/SvrData.cpp index 1f0112f..2c9c31c 100644 --- a/lwrserv/src/SvrData.cpp +++ b/lwrserv/src/SvrData.cpp @@ -70,7 +70,7 @@ friRemote* SvrData::getFriRemote() } void SvrData::updateMessurementData( - VecJoint newJointPos, + Robot::VecJoint newJointPos, MatCarthesian newCartPos, VecTorque newForceAndTorque ) @@ -84,7 +84,7 @@ void SvrData::updateMessurementData( void SvrData::updateMessurement() { - VecJoint newJointPos(0.0f); + Robot::VecJoint newJointPos(0.0f); MatCarthesian newCartPos(0.0f,1.0f); VecTorque newForceAndTorque(0.0f); @@ -92,8 +92,8 @@ void SvrData::updateMessurement() /// update current joint positions friInst->getState(); FRI_STATE state = friInst->getState(); - VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); - VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); + Robot::VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition()); + Robot::VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset()); newJointPos = newJointPosComanded + newJointPosOffset; newJointPos = newJointPos * ( 180.0f / M_PI); @@ -118,9 +118,9 @@ void SvrData::unlock() pthread_mutex_unlock(&dataLock); } -VecJoint SvrData::getMessuredJointPos() +Robot::VecJoint SvrData::getMessuredJointPos() { - VecJoint buf; + Robot::VecJoint buf; pthread_mutex_lock(&dataLock); buf = messured.jointPos; pthread_mutex_unlock(&dataLock); @@ -157,15 +157,15 @@ int SvrData::getMessuredJacobian(float* data, size_t size) return 0; } -VecJoint SvrData::getCommandedJointPos() +Robot::VecJoint SvrData::getCommandedJointPos() { - VecJoint buff; + Robot::VecJoint buff; pthread_mutex_lock(&dataLock); buff = commanded.jointPos; pthread_mutex_unlock(&dataLock); return buff; } -void SvrData::setCommandedJointPos(VecJoint newJointPos) +void SvrData::setCommandedJointPos(Robot::VecJoint newJointPos) { pthread_mutex_lock(&dataLock); commanded.jointPos = newJointPos; @@ -187,41 +187,41 @@ void SvrData::setCommandedCartPos(MatCarthesian newCartPos) } -VecJoint SvrData::getMaxTorque() +Robot::VecJoint SvrData::getMaxTorque() { - VecJoint buff; + Robot::VecJoint buff; pthread_mutex_lock(&dataLock); buff = robot.max.torque; pthread_mutex_unlock(&dataLock); return buff; } -VecJoint SvrData::getMaxAcceleration() +Robot::VecJoint SvrData::getMaxAcceleration() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.accelaration; pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getMaxVelocity() +Robot::VecJoint SvrData::getMaxVelocity() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.velocity; pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getMaxRange() +Robot::VecJoint SvrData::getMaxRange() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = robot.max.range; pthread_mutex_unlock(&dataLock); return retval; } -bool SvrData::checkJointRange(VecJoint vectorToCheck) +bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck) { // save temporal the max range for short time lock pthread_mutex_lock(&dataLock); @@ -257,10 +257,10 @@ int SvrData::setRobotVelocityPercentage(float newVelocityPercentage) pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getRobotVelocity() +Robot::VecJoint SvrData::getRobotVelocity() { float percent = 0.0f; - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); percent = robot.currentVelocityPercentage / 100.0f; @@ -292,10 +292,10 @@ int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage) pthread_mutex_unlock(&dataLock); return retval; } -VecJoint SvrData::getRobotAcceleration() +Robot::VecJoint SvrData::getRobotAcceleration() { float percent = 0; - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); percent = robot.currentAccelerationPercentage / 100.0f; @@ -445,15 +445,15 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType) pthread_mutex_unlock(&dataLock); } -class Trajectory* SvrData::createTrajectory(VecJoint newJointPos) +class Trajectory* SvrData::createTrajectory(Robot::VecJoint newJointPos) { //set new target for next trajectory - VecJoint prevJointPos = getCommandedJointPos(); + Robot::VecJoint prevJointPos = getCommandedJointPos(); class Trajectory* newTrajectory; float sampleTimeMs = getSampleTimeMs(); - VecJoint maxJointVelocity = getRobotVelocity(); - VecJoint maxJointAcceleration = getRobotAcceleration(); + Robot::VecJoint maxJointVelocity = getRobotVelocity(); + Robot::VecJoint maxJointAcceleration = getRobotAcceleration(); enum TrajectoryType type = getTrajectoryType(); //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); @@ -465,9 +465,9 @@ class Trajectory* SvrData::createTrajectory(MatCarthesian newJoin { } -VecJoint SvrData::getMeasuredJointPos() +Robot::VecJoint SvrData::getMeasuredJointPos() { - VecJoint retval(0.0f); + Robot::VecJoint retval(0.0f); pthread_mutex_lock(&dataLock); retval = messured.jointPos; pthread_mutex_unlock(&dataLock); diff --git a/lwrserv/src/commands.cpp b/lwrserv/src/commands.cpp index 8e25a2a..4d5327e 100644 --- a/lwrserv/src/commands.cpp +++ b/lwrserv/src/commands.cpp @@ -41,7 +41,7 @@ #include "StringTool.h" -void moveRobotTo(VecJoint newJointPos) +void moveRobotTo(Robot::VecJoint newJointPos) { Trajectory* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); @@ -67,7 +67,7 @@ void getPositionJoints(SocketObject& client, std::string& arg) std::stringstream ss (std::stringstream::in | std::stringstream::out); // get current joint positions from database - VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); + Robot::VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); // assemble command feadback for (int i=0; i< Robot::joints; i++) @@ -133,7 +133,7 @@ void movePTPJoints(SocketObject& client, std::string& arg) std::stringstream ss2f; // convert string to joint vector - VecJoint newJointPos(0.0f); + Robot::VecJoint newJointPos(0.0f); int i = 0; while (ss >> buf) { @@ -218,7 +218,7 @@ void moveHomRowWiseStatus(SocketObject& client, std::string& arg) configurationParameter.j1os = temp[14] == 1.0f; //Backward Calculation - VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); + Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); //check for joint range if (!SvrData::getInstance()->checkJointRange(newJointPos)) @@ -329,7 +329,7 @@ void startPotFieldMode(SocketObject& client, std::string& arg) (void) arg; //Set current Joint Vals - VecJoint currJoints = SvrData::getInstance()->getMessuredJointPos(); + Robot::VecJoint currJoints = SvrData::getInstance()->getMessuredJointPos(); SvrData::getInstance()->setTrajectoryPotfieldMode(true); moveRobotTo(currJoints); diff --git a/lwrserv/src/main.cpp b/lwrserv/src/main.cpp index 8b62c04..70af2e4 100644 --- a/lwrserv/src/main.cpp +++ b/lwrserv/src/main.cpp @@ -20,7 +20,7 @@ void *threadRobotMovement(void *arg) Trajectory* currentTrajectory = NULL; enum MovementType currentMovementType = MovementJointBased; - VecJoint currentJointPos(0.0f); + Robot::VecJoint currentJointPos(0.0f); MatCarthesian currentCartPos(0.0f,1.0f); friRemote* friInst = SvrData::getInstance()->getFriRemote(); @@ -120,7 +120,7 @@ end: { // send the new joint values float newJointPosToSend[Robot::joints] = {0.0f}; - VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); + Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); newJointPosToSendRad.getData(newJointPosToSend); if (REAL_ROBOT) friInst->doPositionControl(newJointPosToSend);