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
Split 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"
typedef Vec<float, FRI_CART_VEC> VecTorque;
#define jointNumber LBR_MNJ
typedef Vec<float,jointNumber> VecJoint;
class Robot
{
public:
static const unsigned int joints = LBR_MNJ;
typedef Vec<float,jointNumber> VecJoint;
typedef Vec<float,joints> VecJoint;
static int getJointCount(void)
{
@ -51,5 +50,6 @@ class Robot
static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config );
static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config );
};
typedef Vec<float,Robot::joints> VecJoint;
#endif

14
lwrserv/include/SvrData.h
View File

@ -22,7 +22,7 @@ private:
VecJoint jointPos;
MatCarthesian cartPos;
VecTorque forceAndTorque;
float jacobian[FRI_CART_VEC * jointNumber];
float jacobian[FRI_CART_VEC * Robot::joints];
} messured;
struct {
VecJoint jointPos;
@ -44,7 +44,7 @@ private:
float sampleTimeMs;
bool cancel;
bool done;
std::queue< Trajectory<jointNumber>* > list;
std::queue< Trajectory<Robot::joints>* > list;
enum TrajectoryType type;
bool potfieldmode;
}trajectory;
@ -113,8 +113,8 @@ public:
int setSampleTimeMs(float newSampleTime);
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();
void trajectoryCancelDone();
@ -123,10 +123,10 @@ public:
enum TrajectoryType getTrajectoryType();
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 maxJointAcceleration,
VecJoint jointStart,

22
lwrserv/src/SvrData.cpp
View File

@ -26,7 +26,7 @@ SvrData::SvrData(void)
robot.currentVelocityPercentage = 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.accelaration(i) = maxAccJnt[i];
@ -230,7 +230,7 @@ bool SvrData::checkJointRange(VecJoint vectorToCheck)
pthread_mutex_unlock(&dataLock);
// 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)))
{
@ -317,7 +317,7 @@ unsigned int SvrData::getJoints()
unsigned int retval = 0;
pthread_mutex_lock(&dataLock);
retval = jointNumber;
retval = Robot::joints;
pthread_mutex_unlock(&dataLock);
return retval;
}
@ -346,9 +346,9 @@ int SvrData::setSampleTimeMs(float newSampleTimeMs)
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);
if (!trajectory.list.empty() )
{
@ -363,7 +363,7 @@ class Trajectory<jointNumber>* SvrData::popTrajectory()
pthread_mutex_unlock(&dataLock);
return retval;
}
int SvrData::pushTrajectory(class Trajectory<jointNumber>* newTrajectory)
int SvrData::pushTrajectory(class Trajectory<Robot::joints>* newTrajectory)
{
if (newTrajectory == NULL)
return -1;
@ -392,7 +392,7 @@ bool SvrData::cancelCurrentTrajectory()
// clear trajectory list
while(!trajectory.list.empty())
{
class Trajectory<jointNumber>* currentTrajectory = trajectory.list.front();
class Trajectory<Robot::joints>* currentTrajectory = trajectory.list.front();
if(currentTrajectory != NULL)
delete currentTrajectory;
trajectory.list.pop();
@ -445,11 +445,11 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType)
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
VecJoint prevJointPos = getCommandedJointPos();
class Trajectory<jointNumber>* newTrajectory;
class Trajectory<Robot::joints>* newTrajectory;
float sampleTimeMs = getSampleTimeMs();
VecJoint maxJointVelocity = getRobotVelocity();
@ -457,11 +457,11 @@ class Trajectory<jointNumber>* SvrData::createTrajectory(VecJoint newJointPos)
enum TrajectoryType type = getTrajectoryType();
//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;
}
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)
{
Trajectory<jointNumber>* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
Trajectory<Robot::joints>* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
// add trajectory to queue for sender thread
SvrData::getInstance()->pushTrajectory(newTrajectory);
@ -70,7 +70,7 @@ void getPositionJoints(SocketObject& client, std::string& arg)
VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
// assemble command feadback
for (int i=0; i< jointNumber; i++)
for (int i=0; i< Robot::joints; i++)
{
ss.str("");
ss << (jointPos(i)*180/M_PI);
@ -138,7 +138,7 @@ void movePTPJoints(SocketObject& client, std::string& arg)
while (ss >> buf)
{
// to many joint values
if (i>=jointNumber)
if (i>=Robot::joints)
{
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return;
@ -151,7 +151,7 @@ void movePTPJoints(SocketObject& client, std::string& arg)
}
// to less joint values
if (i!=jointNumber)
if (i!=Robot::joints)
{
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return ;

6
lwrserv/src/main.cpp
View File

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

Write Preview
Loading…
Cancel
Save