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

add velocity and accel functions for svrdata

master
philipp schoenberger 10 years ago
parent
478dafc940
commit
cb070b47cc
2 changed files with 130 additions and 13 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. 119
      lwrserv/SvrData.cpp
  2. 24
      lwrserv/include/SvrData.h

119
lwrserv/SvrData.cpp
View File

@ -9,17 +9,18 @@ SvrData* SvrData::single = 0;
SvrData::SvrData(void)
{
velocity = 20;
accelaration = 10;
robot.joints = LBR_MNJ;
// 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};
robot.currentVelocity = 20;
robot.currentAcceleration = 10;
robot.joints = LBR_MNJ;
for (int i = 0; i < robot.joints; ++i)
for (unsigned int i = 0; i < robot.joints; ++i)
{
robot.max.velocity[i] = maxVelJnt[i];
robot.max.accelaration[i] = maxAccJnt[i];
@ -71,6 +72,15 @@ int SvrData::getMessuredJointPos(float* data, size_t size)
return 0;
}
Vec<float,LBR_MNJ> SvrData::getMessuredJointPos()
{
Vec<float,LBR_MNJ> buf ;
pthread_mutex_lock(&dataLock);
buf = messured.jointPos;
pthread_mutex_unlock(&dataLock);
return buf;
}
int SvrData::getMessuredCartPos (float* data, size_t size)
{
if (data == NULL)
@ -84,6 +94,15 @@ int SvrData::getMessuredCartPos (float* data, size_t size)
return 0 ;
}
Mat<float,LBR_MNJ> SvrData::getMessuredCartPos()
{
Mat<float,LBR_MNJ> buf ;
pthread_mutex_lock(&dataLock);
buf = messured.cartPos;
pthread_mutex_unlock(&dataLock);
return buf;
}
int SvrData::getMessuredJacobian(float* data, size_t size)
{
if (data == NULL)
@ -123,7 +142,14 @@ int SvrData::getCommandedJointPos(float* data, size_t size)
return 0;
}
Vec<float, LBR_MNJ> SvrData::getCommandedJointPos()
{
Vec<float, LBR_MNJ> buff;
pthread_mutex_lock(&dataLock);
buff = commanded.jointPos;
pthread_mutex_unlock(&dataLock);
return buff;
}
int SvrData::getCommandedCartPos (float* data, size_t size)
{
if (data == NULL)
@ -137,6 +163,14 @@ int SvrData::getCommandedCartPos (float* data, size_t size)
return 0;
}
Mat<float, LBR_MNJ> SvrData::getCommandedCartPos ()
{
Mat<float, LBR_MNJ> buff;
pthread_mutex_lock(&dataLock);
buff = commanded.cartPos;
pthread_mutex_unlock(&dataLock);
return buff;
}
float SvrData::getMaxTorque(unsigned int pos)
{
@ -194,6 +228,37 @@ Vec<float,LBR_MNJ> SvrData::getMaxAcceleration()
pthread_mutex_unlock(&dataLock);
return buff;
}
float SvrData::getMaxVelocity(unsigned int pos)
{
float retval = -1.0f;
if (pos >= robot.joints)
return retval;
pthread_mutex_lock(&dataLock);
retval = robot.max.velocity[pos-1];
pthread_mutex_unlock(&dataLock);
return retval;
}
int SvrData::getMaxVelocity(float* data, size_t size)
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(float)* robot.joints)
return -EINVAL;
pthread_mutex_lock(&dataLock);
memcpy(data,robot.max.velocity,size);
pthread_mutex_unlock(&dataLock);
return 0;
}
Vec<float,LBR_MNJ> SvrData::getMaxVelocity()
{
Vec<float,LBR_MNJ> buff;
pthread_mutex_lock(&dataLock);
buff = robot.max.velocity;
pthread_mutex_unlock(&dataLock);
return buff;
}
float SvrData::getMaxRange(unsigned int pos)
{
@ -219,7 +284,51 @@ int SvrData::getMaxRange(float* data, size_t size)
pthread_mutex_unlock(&dataLock);
return 0;
}
Vec<float, LBR_MNJ> SvrData::getMaxRange()
{
Vec<float, LBR_MNJ> retval ;
pthread_mutex_lock(&dataLock);
retval = robot.max.range;
pthread_mutex_unlock(&dataLock);
return 0;
}
int SvrData::setRobotVelocity(float newVelocity)
{
unsigned int retval = 0;
pthread_mutex_lock(&dataLock);
robot.currentVelocity = newVelocity;
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotVelocity()
{
float retval = 0;
pthread_mutex_lock(&dataLock);
retval = robot.currentVelocity;
pthread_mutex_unlock(&dataLock);
return retval;
}
int SvrData::setRobotAcceleration(float newAcceleration)
{
unsigned int retval = 0;
pthread_mutex_lock(&dataLock);
robot.currentAcceleration = newAcceleration;
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotAcceleration()
{
float retval = 0;
pthread_mutex_lock(&dataLock);
retval = robot.currentAcceleration;
pthread_mutex_unlock(&dataLock);
return retval;
}
unsigned int SvrData::getJoints()
{
unsigned int retval = 0;

24
lwrserv/include/SvrData.h
View File

@ -25,9 +25,6 @@ private:
float pose[3][4]; //TODO what is this for
} commanded;
float velocity;
float accelaration;
struct {
struct {
float velocity[LBR_MNJ];
@ -36,6 +33,8 @@ private:
float range[LBR_MNJ];
} max;
unsigned int joints;
float currentVelocity;
float currentAcceleration;
} robot;
pthread_mutex_t dataLock;
@ -54,12 +53,21 @@ public:
void unlock();
int getMessuredJointPos(float* data, size_t size);
Vec<float,LBR_MNJ> getMessuredJointPos();
int getMessuredCartPos (float* data, size_t size);
Mat<float,LBR_MNJ> getMessuredCartPos();
int getMessuredJacobian(float* data, size_t size);
int getMessuredForceTorque(float* data, size_t size);
Vec<float,LBR_MNJ> getMessuredForceTorque();
int getCommandedJointPos(float* data, size_t size);
Vec<float,LBR_MNJ> getCommandedJointPos();
int getCommandedCartPos (float* data, size_t size);
Mat<float,LBR_MNJ> getCommandedCartPos();
float getMaxTorque(unsigned int pos);
int getMaxTorque(float* data, size_t size);
@ -77,12 +85,12 @@ public:
int getMaxRange(float* data, size_t size);
Vec<float,LBR_MNJ> getMaxRange();
unsigned int getJoints();
float getRobotVelocity();
int setRobotVelocity(float newVelocity);
float getTrajectoryVelocity();
int setTrajectoryVelocity(float);
float getRobotAcceleration();
int setRobotAcceleration(float newVelocity);
float getTrajectoryAccelaration();
float setTrajectoryAccelaration(float);
unsigned int getJoints();
};
#endif
Write Preview
Loading…
Cancel
Save