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

use one vecjoint defined in robot.h

master
philipp schoenberger 10 years ago
parent
2af0283ad0
commit
9d91690365
6 changed files with 64 additions and 69 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. 1
      lwrserv/include/Robot.h
  2. 48
      lwrserv/include/SvrData.h
  3. 14
      lwrserv/include/lwr4.h
  4. 56
      lwrserv/src/SvrData.cpp
  5. 10
      lwrserv/src/commands.cpp
  6. 4
      lwrserv/src/main.cpp

1
lwrserv/include/Robot.h
View File

@ -50,6 +50,5 @@ 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

48
lwrserv/include/SvrData.h
View File

@ -19,22 +19,22 @@ class SvrData: public Singleton
{ {
private: private:
struct { struct {
VecJoint jointPos;
Robot::VecJoint jointPos;
MatCarthesian cartPos; MatCarthesian cartPos;
VecTorque forceAndTorque; VecTorque forceAndTorque;
float jacobian[FRI_CART_VEC * Robot::joints]; float jacobian[FRI_CART_VEC * Robot::joints];
} messured; } messured;
struct { struct {
VecJoint jointPos;
Robot::VecJoint jointPos;
MatCarthesian cartPos; MatCarthesian cartPos;
} commanded; } commanded;
struct { struct {
struct { struct {
VecJoint velocity;
VecJoint accelaration;
VecJoint torque;
VecJoint range;
Robot::VecJoint velocity;
Robot::VecJoint accelaration;
Robot::VecJoint torque;
Robot::VecJoint range;
} max; } max;
float currentVelocityPercentage; float currentVelocityPercentage;
float currentAccelerationPercentage; float currentAccelerationPercentage;
@ -61,7 +61,7 @@ private:
void updateMessurementData( void updateMessurementData(
VecJoint newJointPos,
Robot::VecJoint newJointPos,
MatCarthesian newCartPos, MatCarthesian newCartPos,
VecTorque newForceAndTorque VecTorque newForceAndTorque
); );
@ -77,32 +77,32 @@ public:
void lock(); void lock();
void unlock(); void unlock();
VecJoint getMeasuredJointPos();
Robot::VecJoint getMeasuredJointPos();
MatCarthesian getMeasuredCartPos(); MatCarthesian getMeasuredCartPos();
int getMessuredJacobian(float* data, size_t size); int getMessuredJacobian(float* data, size_t size);
VecTorque getMessuredForceTorque(); VecTorque getMessuredForceTorque();
VecJoint getMessuredJointPos();
Robot::VecJoint getMessuredJointPos();
MatCarthesian getMessuredCartPos(); MatCarthesian getMessuredCartPos();
VecJoint getCommandedJointPos();
void setCommandedJointPos(VecJoint newJointPos);
Robot::VecJoint getCommandedJointPos();
void setCommandedJointPos(Robot::VecJoint newJointPos);
MatCarthesian getCommandedCartPos(); MatCarthesian getCommandedCartPos();
void setCommandedCartPos(MatCarthesian newCartPos); 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(); float getRobotVelocityPercentage();
int setRobotVelocityPercentage(float newVelocityPercentage); int setRobotVelocityPercentage(float newVelocityPercentage);
VecJoint getRobotAcceleration();
Robot::VecJoint getRobotAcceleration();
float getRobotAccelerationPercentage(); float getRobotAccelerationPercentage();
int setRobotAccelerationPercentage(float newAccelerationPercentage); int setRobotAccelerationPercentage(float newAccelerationPercentage);
@ -123,16 +123,16 @@ public:
enum TrajectoryType getTrajectoryType(); enum TrajectoryType getTrajectoryType();
void setTrajectoryType(enum TrajectoryType newType); void setTrajectoryType(enum TrajectoryType newType);
class Trajectory<Robot::joints>* createTrajectory(VecJoint newJointPos);
class Trajectory<Robot::joints>* createTrajectory(Robot::VecJoint newJointPos);
class Trajectory<Robot::joints>* createTrajectory(MatCarthesian newJointPos); class Trajectory<Robot::joints>* createTrajectory(MatCarthesian newJointPos);
class Trajectory<Robot::joints>* calculateTrajectory( class Trajectory<Robot::joints>* 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); void setTrajectoryPotfieldMode(bool newstate);
bool getTrajectoryPotfieldMode(); bool getTrajectoryPotfieldMode();

14
lwrserv/include/lwr4.h
View File

@ -10,18 +10,14 @@
class LWR4 : public Robot class LWR4 : public Robot
{ {
public : public :
const static int joints = 7;
typedef Vec<float,joints> 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<float, 4> getDhParameter(unsigned int joint); static Vec<float, 4> 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 ) Robot::VecJoint LWR4::backwardCalc(MatCarthesian M , struct RobotConfig config )

56
lwrserv/src/SvrData.cpp
View File

@ -70,7 +70,7 @@ friRemote* SvrData::getFriRemote()
} }
void SvrData::updateMessurementData( void SvrData::updateMessurementData(
VecJoint newJointPos,
Robot::VecJoint newJointPos,
MatCarthesian newCartPos, MatCarthesian newCartPos,
VecTorque newForceAndTorque VecTorque newForceAndTorque
) )
@ -84,7 +84,7 @@ void SvrData::updateMessurementData(
void SvrData::updateMessurement() void SvrData::updateMessurement()
{ {
VecJoint newJointPos(0.0f);
Robot::VecJoint newJointPos(0.0f);
MatCarthesian newCartPos(0.0f,1.0f); MatCarthesian newCartPos(0.0f,1.0f);
VecTorque newForceAndTorque(0.0f); VecTorque newForceAndTorque(0.0f);
@ -92,8 +92,8 @@ void SvrData::updateMessurement()
/// update current joint positions /// update current joint positions
friInst->getState(); friInst->getState();
FRI_STATE state = 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 = newJointPosComanded + newJointPosOffset;
newJointPos = newJointPos * ( 180.0f / M_PI); newJointPos = newJointPos * ( 180.0f / M_PI);
@ -118,9 +118,9 @@ void SvrData::unlock()
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
} }
VecJoint SvrData::getMessuredJointPos()
Robot::VecJoint SvrData::getMessuredJointPos()
{ {
VecJoint buf;
Robot::VecJoint buf;
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
buf = messured.jointPos; buf = messured.jointPos;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
@ -157,15 +157,15 @@ int SvrData::getMessuredJacobian(float* data, size_t size)
return 0; return 0;
} }
VecJoint SvrData::getCommandedJointPos()
Robot::VecJoint SvrData::getCommandedJointPos()
{ {
VecJoint buff;
Robot::VecJoint buff;
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
buff = commanded.jointPos; buff = commanded.jointPos;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return buff; return buff;
} }
void SvrData::setCommandedJointPos(VecJoint newJointPos)
void SvrData::setCommandedJointPos(Robot::VecJoint newJointPos)
{ {
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
commanded.jointPos = newJointPos; 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); pthread_mutex_lock(&dataLock);
buff = robot.max.torque; buff = robot.max.torque;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return buff; return buff;
} }
VecJoint SvrData::getMaxAcceleration()
Robot::VecJoint SvrData::getMaxAcceleration()
{ {
VecJoint retval(0.0f);
Robot::VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
retval = robot.max.accelaration; retval = robot.max.accelaration;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
VecJoint SvrData::getMaxVelocity()
Robot::VecJoint SvrData::getMaxVelocity()
{ {
VecJoint retval(0.0f);
Robot::VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
retval = robot.max.velocity; retval = robot.max.velocity;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
VecJoint SvrData::getMaxRange()
Robot::VecJoint SvrData::getMaxRange()
{ {
VecJoint retval(0.0f);
Robot::VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
retval = robot.max.range; retval = robot.max.range;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
bool SvrData::checkJointRange(VecJoint vectorToCheck)
bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck)
{ {
// save temporal the max range for short time lock // save temporal the max range for short time lock
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
@ -257,10 +257,10 @@ int SvrData::setRobotVelocityPercentage(float newVelocityPercentage)
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
VecJoint SvrData::getRobotVelocity()
Robot::VecJoint SvrData::getRobotVelocity()
{ {
float percent = 0.0f; float percent = 0.0f;
VecJoint retval(0.0f);
Robot::VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
percent = robot.currentVelocityPercentage / 100.0f; percent = robot.currentVelocityPercentage / 100.0f;
@ -292,10 +292,10 @@ int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage)
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
VecJoint SvrData::getRobotAcceleration()
Robot::VecJoint SvrData::getRobotAcceleration()
{ {
float percent = 0; float percent = 0;
VecJoint retval(0.0f);
Robot::VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
percent = robot.currentAccelerationPercentage / 100.0f; percent = robot.currentAccelerationPercentage / 100.0f;
@ -445,15 +445,15 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType)
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
} }
class Trajectory<Robot::joints>* SvrData::createTrajectory(VecJoint newJointPos)
class Trajectory<Robot::joints>* SvrData::createTrajectory(Robot::VecJoint newJointPos)
{ {
//set new target for next trajectory //set new target for next trajectory
VecJoint prevJointPos = getCommandedJointPos();
Robot::VecJoint prevJointPos = getCommandedJointPos();
class Trajectory<Robot::joints>* newTrajectory; class Trajectory<Robot::joints>* newTrajectory;
float sampleTimeMs = getSampleTimeMs(); float sampleTimeMs = getSampleTimeMs();
VecJoint maxJointVelocity = getRobotVelocity();
VecJoint maxJointAcceleration = getRobotAcceleration();
Robot::VecJoint maxJointVelocity = getRobotVelocity();
Robot::VecJoint maxJointAcceleration = getRobotAcceleration();
enum TrajectoryType type = getTrajectoryType(); enum TrajectoryType type = getTrajectoryType();
//newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); //newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
@ -465,9 +465,9 @@ class Trajectory<Robot::joints>* SvrData::createTrajectory(MatCarthesian newJoin
{ {
} }
VecJoint SvrData::getMeasuredJointPos()
Robot::VecJoint SvrData::getMeasuredJointPos()
{ {
VecJoint retval(0.0f);
Robot::VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
retval = messured.jointPos; retval = messured.jointPos;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);

10
lwrserv/src/commands.cpp
View File

@ -41,7 +41,7 @@
#include "StringTool.h" #include "StringTool.h"
void moveRobotTo(VecJoint newJointPos)
void moveRobotTo(Robot::VecJoint newJointPos)
{ {
Trajectory<Robot::joints>* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos); Trajectory<Robot::joints>* 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); std::stringstream ss (std::stringstream::in | std::stringstream::out);
// get current joint positions from database // get current joint positions from database
VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
Robot::VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
// assemble command feadback // assemble command feadback
for (int i=0; i< Robot::joints; i++) for (int i=0; i< Robot::joints; i++)
@ -133,7 +133,7 @@ void movePTPJoints(SocketObject& client, std::string& arg)
std::stringstream ss2f; std::stringstream ss2f;
// convert string to joint vector // convert string to joint vector
VecJoint newJointPos(0.0f);
Robot::VecJoint newJointPos(0.0f);
int i = 0; int i = 0;
while (ss >> buf) while (ss >> buf)
{ {
@ -218,7 +218,7 @@ void moveHomRowWiseStatus(SocketObject& client, std::string& arg)
configurationParameter.j1os = temp[14] == 1.0f; configurationParameter.j1os = temp[14] == 1.0f;
//Backward Calculation //Backward Calculation
VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter);
Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter);
//check for joint range //check for joint range
if (!SvrData::getInstance()->checkJointRange(newJointPos)) if (!SvrData::getInstance()->checkJointRange(newJointPos))
@ -329,7 +329,7 @@ void startPotFieldMode(SocketObject& client, std::string& arg)
(void) arg; (void) arg;
//Set current Joint Vals //Set current Joint Vals
VecJoint currJoints = SvrData::getInstance()->getMessuredJointPos();
Robot::VecJoint currJoints = SvrData::getInstance()->getMessuredJointPos();
SvrData::getInstance()->setTrajectoryPotfieldMode(true); SvrData::getInstance()->setTrajectoryPotfieldMode(true);
moveRobotTo(currJoints); moveRobotTo(currJoints);

4
lwrserv/src/main.cpp
View File

@ -20,7 +20,7 @@ void *threadRobotMovement(void *arg)
Trajectory<Robot::joints>* currentTrajectory = NULL; Trajectory<Robot::joints>* currentTrajectory = NULL;
enum MovementType currentMovementType = MovementJointBased; enum MovementType currentMovementType = MovementJointBased;
VecJoint currentJointPos(0.0f);
Robot::VecJoint currentJointPos(0.0f);
MatCarthesian currentCartPos(0.0f,1.0f); MatCarthesian currentCartPos(0.0f,1.0f);
friRemote* friInst = SvrData::getInstance()->getFriRemote(); friRemote* friInst = SvrData::getInstance()->getFriRemote();
@ -120,7 +120,7 @@ end:
{ {
// send the new joint values // send the new joint values
float newJointPosToSend[Robot::joints] = {0.0f}; float newJointPosToSend[Robot::joints] = {0.0f};
VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI);
Robot::VecJoint newJointPosToSendRad = currentJointPos*(1./180*M_PI);
newJointPosToSendRad.getData(newJointPosToSend); newJointPosToSendRad.getData(newJointPosToSend);
if (REAL_ROBOT) if (REAL_ROBOT)
friInst->doPositionControl(newJointPosToSend); friInst->doPositionControl(newJointPosToSend);

Write Preview
Loading…
Cancel
Save