phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

use one joint defined in Robot class

master
philipp schoenberger 10 years ago
parent
0554e74618
commit
2af0283ad0
6 changed files with 28 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. BIN
      lwrserv/.ycm_extra_conf.pyc
  2. 6
      lwrserv/include/Robot.h
  3. 14
      lwrserv/include/SvrData.h
  4. 22
      lwrserv/src/SvrData.cpp
  5. 8
      lwrserv/src/commands.cpp
  6. 6
      lwrserv/src/main.cpp

BIN
lwrserv/.ycm_extra_conf.pyc
View File

6
lwrserv/include/Robot.h
View File

@ -10,13 +10,12 @@
#include "Trajectroy.h" #include "Trajectroy.h"
typedef Vec<float, FRI_CART_VEC> VecTorque; typedef Vec<float, FRI_CART_VEC> VecTorque;
#define jointNumber LBR_MNJ
typedef Vec<float,jointNumber> VecJoint;
class Robot class Robot
{ {
public: public:
static const unsigned int joints = LBR_MNJ;
typedef Vec<float,jointNumber> VecJoint;
typedef Vec<float,joints> VecJoint;
static int getJointCount(void) static int getJointCount(void)
{ {
@ -51,5 +50,6 @@ class Robot
static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config ); static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config );
static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config );
}; };
typedef Vec<float,Robot::joints> VecJoint;
#endif #endif

14
lwrserv/include/SvrData.h
View File

@ -22,7 +22,7 @@ private:
VecJoint jointPos; VecJoint jointPos;
MatCarthesian cartPos; MatCarthesian cartPos;
VecTorque forceAndTorque; VecTorque forceAndTorque;
float jacobian[FRI_CART_VEC * jointNumber];
float jacobian[FRI_CART_VEC * Robot::joints];
} messured; } messured;
struct { struct {
VecJoint jointPos; VecJoint jointPos;
@ -44,7 +44,7 @@ private:
float sampleTimeMs; float sampleTimeMs;
bool cancel; bool cancel;
bool done; bool done;
std::queue< Trajectory<jointNumber>* > list;
std::queue< Trajectory<Robot::joints>* > list;
enum TrajectoryType type; enum TrajectoryType type;
bool potfieldmode; bool potfieldmode;
}trajectory; }trajectory;
@ -113,8 +113,8 @@ public:
int setSampleTimeMs(float newSampleTime); int setSampleTimeMs(float newSampleTime);
bool getTrajectoryCancel(); bool getTrajectoryCancel();
class Trajectory<jointNumber>* popTrajectory();
int pushTrajectory(class Trajectory<jointNumber>* newFrajectory);
class Trajectory<Robot::joints>* popTrajectory();
int pushTrajectory(class Trajectory<Robot::joints>* newFrajectory);
bool cancelCurrentTrajectory(); bool cancelCurrentTrajectory();
void trajectoryCancelDone(); void trajectoryCancelDone();
@ -123,10 +123,10 @@ public:
enum TrajectoryType getTrajectoryType(); enum TrajectoryType getTrajectoryType();
void setTrajectoryType(enum TrajectoryType newType); void setTrajectoryType(enum TrajectoryType newType);
class Trajectory<jointNumber>* createTrajectory(VecJoint newJointPos);
class Trajectory<jointNumber>* createTrajectory(MatCarthesian newJointPos);
class Trajectory<Robot::joints>* createTrajectory(VecJoint newJointPos);
class Trajectory<Robot::joints>* createTrajectory(MatCarthesian newJointPos);
class Trajectory<jointNumber>* calculateTrajectory(
class Trajectory<Robot::joints>* calculateTrajectory(
VecJoint maxJointVelocity, VecJoint maxJointVelocity,
VecJoint maxJointAcceleration, VecJoint maxJointAcceleration,
VecJoint jointStart, VecJoint jointStart,

22
lwrserv/src/SvrData.cpp
View File

@ -26,7 +26,7 @@ SvrData::SvrData(void)
robot.currentVelocityPercentage = 10; robot.currentVelocityPercentage = 10;
robot.currentAccelerationPercentage = 10; robot.currentAccelerationPercentage = 10;
for (unsigned int i = 0; i < jointNumber; ++i)
for (unsigned int i = 0; i < Robot::joints; ++i)
{ {
robot.max.velocity(i) = maxVelJnt[i]; robot.max.velocity(i) = maxVelJnt[i];
robot.max.accelaration(i) = maxAccJnt[i]; robot.max.accelaration(i) = maxAccJnt[i];
@ -230,7 +230,7 @@ bool SvrData::checkJointRange(VecJoint vectorToCheck)
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
// check if the value is in plus or minus range // check if the value is in plus or minus range
for (int i = 0; i<jointNumber; i++)
for (int i = 0; i<Robot::joints; i++)
{ {
if (abs(vectorToCheck(i)) > abs(maxJointRange(i))) if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
{ {
@ -317,7 +317,7 @@ unsigned int SvrData::getJoints()
unsigned int retval = 0; unsigned int retval = 0;
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
retval = jointNumber;
retval = Robot::joints;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
@ -346,9 +346,9 @@ int SvrData::setSampleTimeMs(float newSampleTimeMs)
return retval; return retval;
} }
class Trajectory<jointNumber>* SvrData::popTrajectory()
class Trajectory<Robot::joints>* SvrData::popTrajectory()
{ {
class Trajectory<jointNumber>* retval = NULL;
class Trajectory<Robot::joints>* retval = NULL;
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
if (!trajectory.list.empty() ) if (!trajectory.list.empty() )
{ {
@ -363,7 +363,7 @@ class Trajectory<jointNumber>* SvrData::popTrajectory()
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
int SvrData::pushTrajectory(class Trajectory<jointNumber>* newTrajectory)
int SvrData::pushTrajectory(class Trajectory<Robot::joints>* newTrajectory)
{ {
if (newTrajectory == NULL) if (newTrajectory == NULL)
return -1; return -1;
@ -392,7 +392,7 @@ bool SvrData::cancelCurrentTrajectory()
// clear trajectory list // clear trajectory list
while(!trajectory.list.empty()) while(!trajectory.list.empty())
{ {
class Trajectory<jointNumber>* currentTrajectory = trajectory.list.front();
class Trajectory<Robot::joints>* currentTrajectory = trajectory.list.front();
if(currentTrajectory != NULL) if(currentTrajectory != NULL)
delete currentTrajectory; delete currentTrajectory;
trajectory.list.pop(); trajectory.list.pop();
@ -445,11 +445,11 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType)
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
} }
class Trajectory<jointNumber>* SvrData::createTrajectory(VecJoint newJointPos)
class Trajectory<Robot::joints>* SvrData::createTrajectory(VecJoint newJointPos)
{ {
//set new target for next trajectory //set new target for next trajectory
VecJoint prevJointPos = getCommandedJointPos(); VecJoint prevJointPos = getCommandedJointPos();
class Trajectory<jointNumber>* newTrajectory;
class Trajectory<Robot::joints>* newTrajectory;
float sampleTimeMs = getSampleTimeMs(); float sampleTimeMs = getSampleTimeMs();
VecJoint maxJointVelocity = getRobotVelocity(); VecJoint maxJointVelocity = getRobotVelocity();
@ -457,11 +457,11 @@ class Trajectory<jointNumber>* SvrData::createTrajectory(VecJoint newJointPos)
enum TrajectoryType type = getTrajectoryType(); enum TrajectoryType type = getTrajectoryType();
//newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
newTrajectory = Trajectory<jointNumber>::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
newTrajectory = Trajectory<Robot::joints>::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
return newTrajectory; return newTrajectory;
} }
class Trajectory<jointNumber>* SvrData::createTrajectory(MatCarthesian newJointPos)
class Trajectory<Robot::joints>* SvrData::createTrajectory(MatCarthesian newJointPos)
{ {
} }

8
lwrserv/src/commands.cpp
View File

@ -43,7 +43,7 @@
void moveRobotTo(VecJoint newJointPos) void moveRobotTo(VecJoint newJointPos)
{ {
Trajectory<jointNumber>* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
Trajectory<Robot::joints>* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
// add trajectory to queue for sender thread // add trajectory to queue for sender thread
SvrData::getInstance()->pushTrajectory(newTrajectory); SvrData::getInstance()->pushTrajectory(newTrajectory);
@ -70,7 +70,7 @@ void getPositionJoints(SocketObject& client, std::string& arg)
VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
// assemble command feadback // assemble command feadback
for (int i=0; i< jointNumber; i++)
for (int i=0; i< Robot::joints; i++)
{ {
ss.str(""); ss.str("");
ss << (jointPos(i)*180/M_PI); ss << (jointPos(i)*180/M_PI);
@ -138,7 +138,7 @@ void movePTPJoints(SocketObject& client, std::string& arg)
while (ss >> buf) while (ss >> buf)
{ {
// to many joint values // to many joint values
if (i>=jointNumber)
if (i>=Robot::joints)
{ {
client.Send(SVR_INVALID_ARGUMENT_COUNT); client.Send(SVR_INVALID_ARGUMENT_COUNT);
return; return;
@ -151,7 +151,7 @@ void movePTPJoints(SocketObject& client, std::string& arg)
} }
// to less joint values // to less joint values
if (i!=jointNumber)
if (i!=Robot::joints)
{ {
client.Send(SVR_INVALID_ARGUMENT_COUNT); client.Send(SVR_INVALID_ARGUMENT_COUNT);
return ; return ;

6
lwrserv/src/main.cpp
View File

@ -17,7 +17,7 @@ void *threadRobotMovement(void *arg)
// unused parameters // unused parameters
(void) arg; (void) arg;
Trajectory<jointNumber>* currentTrajectory = NULL;
Trajectory<Robot::joints>* currentTrajectory = NULL;
enum MovementType currentMovementType = MovementJointBased; enum MovementType currentMovementType = MovementJointBased;
VecJoint currentJointPos(0.0f); VecJoint currentJointPos(0.0f);
@ -36,7 +36,7 @@ void *threadRobotMovement(void *arg)
SvrData::getInstance()->setCommandedJointPos(currentJointPos); SvrData::getInstance()->setCommandedJointPos(currentJointPos);
float newJointPosToSend[jointNumber] = {0.0f};
float newJointPosToSend[Robot::joints] = {0.0f};
currentJointPos.getData(newJointPosToSend); currentJointPos.getData(newJointPosToSend);
if (REAL_ROBOT) if (REAL_ROBOT)
friInst->doPositionControl(newJointPosToSend); friInst->doPositionControl(newJointPosToSend);
@ -119,7 +119,7 @@ end:
case MovementJointBased: case MovementJointBased:
{ {
// send the new joint values // send the new joint values
float newJointPosToSend[jointNumber] = {0.0f};
float newJointPosToSend[Robot::joints] = {0.0f};
VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI); VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI);
newJointPosToSendRad.getData(newJointPosToSend); newJointPosToSendRad.getData(newJointPosToSend);
if (REAL_ROBOT) if (REAL_ROBOT)

Write Preview
Loading…
Cancel
Save