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

trivial

master
philipp schoenberger 10 years ago
parent
cc471ebb8f
commit
5c12d4de02
8 changed files with 315 additions and 285 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. 20
      lwrserv/LinearTrajectory.cpp
  2. 82
      lwrserv/SvrData.cpp
  3. 98
      lwrserv/SvrHandling.cpp
  4. 22
      lwrserv/Trajectroy.cpp
  5. 3
      lwrserv/friremote.cpp
  6. 14
      lwrserv/include/SvrData.h
  7. 2
      lwrserv/include/Trajectroy.h
  8. 5
      lwrserv/program.cpp

20
lwrserv/LinearTrajectory.cpp
View File

@ -4,17 +4,18 @@
template <unsigned SIZE>
LinearJointTrajectory<SIZE>::LinearJointTrajectory(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement_
float sampleTimeMs,
Vec<float,SIZE> maxJointVelocity,
Vec<float,SIZE> maxJointAcceleration,
Vec<float,SIZE> jointStart,
Vec<float,SIZE> jointEnd
)
{
Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
LinearJointTrajectory(
steps_,
totalTime_,
jointMovement_,
velocity);
//
maxJointLocalVelocity = maxJointVelocity * sampleTimeMs;
maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs;
// calculate sample count
}
template <unsigned SIZE>
@ -25,6 +26,7 @@ LinearJointTrajectory<SIZE>::LinearJointTrajectory(
Vec<float,SIZE> velocity
)
{
Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
super(steps_, totalTime_, jointMovement);
if ( velocity > jointMovement/totalTime_)

82
lwrserv/SvrData.cpp
View File

@ -20,8 +20,8 @@ SvrData::SvrData(void)
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.currentVelocityPercentage = 20;
robot.currentAccelerationPercentage = 10;
for (unsigned int i = 0; i < JOINT_NUMBER; ++i)
{
@ -191,40 +191,38 @@ VecJoint SvrData::getMaxTorque()
VecJoint SvrData::getMaxAcceleration()
{
VecJoint buff;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock);
buff = robot.max.accelaration;
retval = robot.max.accelaration;
pthread_mutex_unlock(&dataLock);
return buff;
return retval;
}
VecJoint SvrData::getMaxVelocity()
{
VecJoint buff;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock);
buff = robot.max.velocity;
retval = robot.max.velocity;
pthread_mutex_unlock(&dataLock);
return buff;
return retval;
}
VecJoint SvrData::getMaxRange()
{
VecJoint buff;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock);
buff = robot.max.range;
retval = robot.max.range;
pthread_mutex_unlock(&dataLock);
return buff;
return retval;
}
bool SvrData::checkJointRange(VecJoint vectorToCheck)
{
bool retval = false;
// save temporal the max range for short time lock
pthread_mutex_lock(&dataLock);
Vec<float,LBR_MNJ> maxJointRange;
maxJointRange = robot.max.range;
pthread_mutex_unlock(&dataLock);
// check if the value is in plus or minus range
for (int i = 0; i<JOINT_NUMBER; i++)
{
if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
@ -234,42 +232,76 @@ bool SvrData::checkJointRange(VecJoint vectorToCheck)
}
}
// no join angle is too big
return false;
return true;
}
int SvrData::setRobotVelocity(float newVelocity)
int SvrData::setRobotVelocityPercentage(float newVelocityPercentage)
{
unsigned int retval = 0;
// percentage is to low
if (newVelocityPercentage <= 0.0f)
return -1;
// percentage is to high
if (newVelocityPercentage > 100.0f)
return -2;
pthread_mutex_lock(&dataLock);
robot.currentVelocity = newVelocity;
robot.currentVelocityPercentage = newVelocityPercentage;
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotVelocity()
VecJoint SvrData::getRobotVelocity()
{
float percent = 0.0f;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock);
percent = robot.currentVelocityPercentage / 100.0f;
retval = robot.max.velocity * percent;
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotVelocityPercentage()
{
float retval = 0;
pthread_mutex_lock(&dataLock);
retval = robot.currentVelocity;
retval = robot.currentVelocityPercentage;
pthread_mutex_unlock(&dataLock);
return retval;
}
int SvrData::setRobotAcceleration(float newAcceleration)
int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage)
{
unsigned int retval = 0;
int retval = 0;
// percentage is to low
if (newAccelerationPercentage <= 0.0f)
return -1;
// percentage is to high
if (newAccelerationPercentage > 100.0f)
return -2;
pthread_mutex_lock(&dataLock);
robot.currentAccelerationPercentage = newAccelerationPercentage;
pthread_mutex_unlock(&dataLock);
return retval;
}
VecJoint SvrData::getRobotAcceleration()
{
float percent = 0;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock);
robot.currentAcceleration = newAcceleration;
percent = robot.currentAccelerationPercentage / 100.0f;
retval = robot.max.accelaration * percent;
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotAcceleration()
float SvrData::getRobotAccelerationPercentage()
{
float retval = 0;
pthread_mutex_lock(&dataLock);
retval = robot.currentAcceleration;
retval = robot.currentAccelerationPercentage;
pthread_mutex_unlock(&dataLock);
return retval;
}

98
lwrserv/SvrHandling.cpp
View File

@ -10,6 +10,7 @@
#include "SvrHandling.h"
#include "SvrData.h"
#include "Trajectroy.h"
#include <boost/thread/thread.hpp>
void printUsage(SocketObject& client)
{
@ -46,7 +47,6 @@ void debugMat(Mat4f M)
double* kukaBackwardCalc(double M_[12], float arg[3])
{
double* Joints = new double[7];
//DH-Parameter
float alp[7] = {-M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, 0.0f};
@ -460,6 +460,7 @@ void SvrHandling::handle(SocketObject& client)
}
}
string SvrHandling::timestamp()
{
time_t rawtime;
@ -480,15 +481,18 @@ void SvrHandling::GetPositionJoints(SocketObject& client)
string out = "";
stringstream ss (stringstream::in | stringstream::out);
// get current joint positions from database
VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
// disassemble command
// assemble command feadback
for (int i=0; i< JOINT_NUMBER; i++)
{
ss.str("");
ss << (jointPos(i)*180/M_PI);
out += ss.str() + " ";
}
// send msg to client
client.Send(out);
}
@ -497,17 +501,21 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client)
string out = "";
stringstream ss (stringstream::in | stringstream::out);
// get current Cartesianpositions from database
MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
// assemble command feadback row wise
for (int i=0; i< FRI_CART_FRM_DIM; i++)
{
int row = i / 4;
int column = i % 4;
int row = i % 4;
int column = i / 4;
ss.str("");
ss << (cartPos(row,column));
out += ss.str() + " ";
}
// send msg to client
client.Send(out);
}
@ -570,33 +578,20 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
class Trajectory<JOINT_NUMBER>* newTrajectory;
// calculate number of movement steps
dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
VecJoint maxJointVelocity = SvrData::getInstance()->getMaxVelocity();
VecJoint maxJointAcceleration = SvrData::getInstance()->getMaxAcceleration();
for (int j=0; j<JOINT_NUMBER; j++)
{
if (lMaxSpeed(j) > deltaAbs(j)/(double)2.0)
{
dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0;
} else
{
dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j);
}
}
int maxSteps = ceil(dGesamt.max());
newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(steps, totalTime, newJointPos);
newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
__MSRCMDJNTPOS = true;
//Wait to end of movement
while (1)
{
if (!__MSRCMDJNTPOS)
while (true)
{
if (SvrData::getInstance()->getMovementDone() == true)
break;
}
boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
}
client.Send(SVR_TRUE_RSP);
}
@ -789,7 +784,8 @@ void SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
client.Send(SVR_TRUE_RSP);
}
void SvrHandling::SetPos(SocketObject& client, string& args){
void SvrHandling::SetPos(SocketObject& client, string& args)
{
string buf;
double temp[15];
@ -868,57 +864,55 @@ void SvrHandling::SetPos(SocketObject& client, string& args){
void SvrHandling::SetJoints(SocketObject& client, string& args)
{
string buf;
double temp[15];
VecJoint newJointPos(0.0f);
stringstream ss(args);
stringstream ss2f;
vector<string> tokens;
int i = 0;
// convert to Joint vector
while (ss >> buf)
{
if (i>LBR_MNJ-1)
// to many input arguments or Joints
if (i>=JOINT_NUMBER)
{
client.Send(SVR_FALSE_RSP);
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return;
}
tokens.push_back(buf);
// convert string to float
ss2f.flush();
ss2f.clear();
ss2f << buf;
ss2f >> temp[i];
ss2f >> newJointPos(i);
i++;
}
if (i<LBR_MNJ-1)
// to less joint values
if (i != JOINT_NUMBER)
{
client.Send(SVR_FALSE_RSP);
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return;
}
//Check for Joint Range
if (!checkJntRange(temp))
if (!checkJntRange(newJointPos))
{
string out = "Error: Joints out of Range!";
client.Send(out);
client.Send(SVR_OUT_OF_RANGE);
return;
}
// Set global Position
__MSRSETPOS = true;
globi = 1;
for (int i=0 ;i<LBR_MNJ; i++)
{
MSRCMDJNTPOS[i]=temp[i];
}
__MSRSETPOS = false;
sleep(5);
globi = 0;
//__MSRCMDJNTPOS = true;
// calculate trajectory from last commanded joint position
VecJoint currentComandedJoint = SvrData::getInstance()->getCommandedJointPos();
////Wait to end of movement
//while (1){
// if (!__MSRCMDJNTPOS){
// break;}
//}
// set new trajectory
// Set new target Position and also calculate cart pos
SvrData::getInstance()->setCommandedJointPos(newJointPos);
// TODO calculate and calculate position in Cartesian coordinates
// wait till position is reached
// return positiv response to client
client.Send(SVR_TRUE_RSP);
}

22
lwrserv/Trajectroy.cpp
View File

@ -4,22 +4,30 @@
template <unsigned SIZE>
Trajectory<SIZE>::Trajectory(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement
float sampleTimeMs,
Vec<float,SIZE> maxJointVelocity,
Vec<float,SIZE> maxJointAcceleration,
Vec<float,SIZE> jointStart,
Vec<float,SIZE> jointEnd
)
{
movementType = MovementJointBased;
steps = steps_;
steps = 1;
currentStep = 0;
totalTime = totalTime_;
totalTime = steps * sampleTimeMs;
nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps);
nodes[0].jointPos = jointEnd;
nodes[0].cartPos = kukaBackwardCalc();
nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs;
nodes[0].acceleration = Vec<float,SIZE>(9999.0f);
// unused
(void) jointMovement;
(void)jointStart;
(void)jointEnd;
}
template <unsigned SIZE>

3
lwrserv/friremote.cpp
View File

@ -242,7 +242,6 @@ int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_
if ( newCartDamp)
{
cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP;
;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.cartDamping[i]=newCartDamp[i];
@ -252,7 +251,6 @@ int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_
if ( newAddTcpFT)
{
cmd.cmd.cmdFlags|=FRI_CMD_TCPFT;
;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.addTcpFT[i]=newAddTcpFT[i];
@ -271,7 +269,6 @@ int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_
cmd.cmd.jntPos[i]=newJntNullspace[i];
}
if (flagDataExchange)
{
return doDataExchange();

14
lwrserv/include/SvrData.h
View File

@ -38,8 +38,8 @@ private:
VecJoint torque;
VecJoint range;
} max;
float currentVelocity;
float currentAcceleration;
float currentVelocityPercentage;
float currentAccelerationPercentage;
} robot;
struct {
@ -97,11 +97,13 @@ public:
bool checkJointRange(VecJoint vectorToCheck);
float getRobotVelocity();
int setRobotVelocity(float newVelocity);
VecJoint getRobotVelocity();
float getRobotVelocityPercentage();
int setRobotVelocityPercentage(float newVelocityPercentage);
float getRobotAcceleration();
int setRobotAcceleration(float newVelocity);
VecJoint getRobotAcceleration();
float getRobotAccelerationPercentage();
int setRobotAccelerationPercentage(float newAccelerationPercentage);
unsigned int getJoints();

2
lwrserv/include/Trajectroy.h
View File

@ -57,7 +57,7 @@ class Trajectory
struct trajectoryNode
{
Vec<float,SIZE> jointPos;
Mat<float,4>* cartPos;
Mat<float,4> cartPos;
Vec<float,SIZE> velocity;
Vec<float,SIZE> acceleration;
};

5
lwrserv/program.cpp
View File

@ -9,11 +9,6 @@
#include "SvrData.h"
#include <boost/thread/thread.hpp>
void printMat(Mat4f M)
{
cout << M << endl;
}
float* abctomat(float a, float b, float c)
{
Mat4f rx;

Write Preview
Loading…
Cancel
Save