philipp schoenberger
10 years ago
11 changed files with 419 additions and 394 deletions
-
BINlwrserv/.ycm_extra_conf.pyc
-
21lwrserv/LinearTrajectory.cpp
-
15lwrserv/SvrData.cpp
-
303lwrserv/SvrHandling.cpp
-
4lwrserv/SvrHandling.h
-
193lwrserv/Trajectroy.cpp
-
2lwrserv/global.cpp
-
60lwrserv/include/LinearTrajectory.h
-
18lwrserv/include/SvrData.h
-
189lwrserv/include/Trajectroy.h
-
8lwrserv/include/vec.h
@ -1,193 +0,0 @@ |
|||||
#include <stdlib.h>
|
|
||||
#include "Trajectroy.h"
|
|
||||
#include "vec.h"
|
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
Trajectory<SIZE>::Trajectory( |
|
||||
float sampleTimeMs, |
|
||||
Vec<float,SIZE> maxJointVelocity, |
|
||||
Vec<float,SIZE> maxJointAcceleration, |
|
||||
Vec<float,SIZE> jointStart, |
|
||||
Vec<float,SIZE> jointEnd |
|
||||
) |
|
||||
{ |
|
||||
movementType = MovementJointBased; |
|
||||
|
|
||||
steps = 1; |
|
||||
currentStep = 0; |
|
||||
|
|
||||
totalTime = steps * sampleTimeMs; |
|
||||
|
|
||||
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)jointStart; |
|
||||
(void)jointEnd; |
|
||||
} |
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
Trajectory<SIZE>::Trajectory( |
|
||||
unsigned int steps_, |
|
||||
float totalTime_, |
|
||||
Mat<float,4> cartStart, |
|
||||
Mat<float,4> cartEnd |
|
||||
) |
|
||||
{ |
|
||||
movementType = MovementCartBased; |
|
||||
|
|
||||
steps = steps_; |
|
||||
currentStep = 0; |
|
||||
|
|
||||
totalTime = totalTime_; |
|
||||
|
|
||||
nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_); |
|
||||
|
|
||||
// unused
|
|
||||
(void) cartStart; |
|
||||
(void) cartEnd; |
|
||||
} |
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
Trajectory<SIZE>::~Trajectory() |
|
||||
{ |
|
||||
free(nodes); |
|
||||
} |
|
||||
|
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
unsigned int Trajectory<SIZE>::getSteps() |
|
||||
{ |
|
||||
return steps; |
|
||||
} |
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
unsigned int Trajectory<SIZE>::getRemainingSteps() |
|
||||
{ |
|
||||
if (steps >= currentStep ) |
|
||||
return steps - currentStep; |
|
||||
else |
|
||||
return 0; |
|
||||
} |
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
unsigned int Trajectory<SIZE>::getCurrentStep() |
|
||||
{ |
|
||||
return currentStep; |
|
||||
} |
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
enum MovementType Trajectory<SIZE>::getMovementType() |
|
||||
{ |
|
||||
return movementType; |
|
||||
} |
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
Vec<float,SIZE> Trajectory<SIZE>::getNextJointPos () |
|
||||
{ |
|
||||
Vec<float,SIZE> retval(0.0f); |
|
||||
unsigned int pos = currentStep; |
|
||||
if (pos >= steps) |
|
||||
{ |
|
||||
pos = steps; |
|
||||
} |
|
||||
|
|
||||
if (nodes != NULL && movementType == MovementJointBased) |
|
||||
{ |
|
||||
retval = nodes[pos].jointPos; |
|
||||
} |
|
||||
|
|
||||
// increment step
|
|
||||
currentStep ++; |
|
||||
|
|
||||
return retval; |
|
||||
} |
|
||||
template <unsigned SIZE> |
|
||||
Mat<float,4> Trajectory<SIZE>::getNextCartPos () |
|
||||
{ |
|
||||
Mat<float,4> retval(0.0f,1.0f); |
|
||||
unsigned int pos = currentStep; |
|
||||
if (pos >= steps) |
|
||||
{ |
|
||||
pos = steps; |
|
||||
} |
|
||||
|
|
||||
if (nodes != NULL && movementType == MovementCartBased) |
|
||||
{ |
|
||||
retval = nodes[pos].cartPos; |
|
||||
} |
|
||||
|
|
||||
// increment step
|
|
||||
currentStep ++; |
|
||||
|
|
||||
return retval; |
|
||||
} |
|
||||
|
|
||||
template <unsigned SIZE> |
|
||||
Vec<float,SIZE> Trajectory<SIZE>::getJointPos (unsigned int step) |
|
||||
{ |
|
||||
Vec<float,SIZE> retval(0.0f); |
|
||||
if (step >= steps) |
|
||||
{ |
|
||||
return retval; |
|
||||
} |
|
||||
|
|
||||
if (nodes != NULL && movementType == MovementJointBased) |
|
||||
{ |
|
||||
retval = nodes[step].cartPos; |
|
||||
} |
|
||||
|
|
||||
return retval; |
|
||||
} |
|
||||
template <unsigned SIZE> |
|
||||
Mat<float,4> Trajectory<SIZE>::getCartPos (unsigned int step) |
|
||||
{ |
|
||||
Mat<float,4> retval(0.0f, 1.0f); |
|
||||
if (step >= steps) |
|
||||
{ |
|
||||
return retval; |
|
||||
} |
|
||||
|
|
||||
if (nodes != NULL && movementType == MovementCartBased) |
|
||||
{ |
|
||||
retval = nodes[step].cartPos; |
|
||||
} |
|
||||
|
|
||||
return retval; |
|
||||
} |
|
||||
template <unsigned SIZE> |
|
||||
Vec<float,SIZE> Trajectory<SIZE>::getJointVelocity (unsigned int step) |
|
||||
{ |
|
||||
Vec<float,SIZE> retval(0.0f); |
|
||||
if (step >= steps) |
|
||||
{ |
|
||||
return retval; |
|
||||
} |
|
||||
|
|
||||
if (nodes != NULL && movementType == MovementCartBased) |
|
||||
{ |
|
||||
retval = nodes[step].velocity; |
|
||||
} |
|
||||
|
|
||||
return retval; |
|
||||
} |
|
||||
template <unsigned SIZE> |
|
||||
Vec<float,SIZE> Trajectory<SIZE>::getJointAcceleration(unsigned int step) |
|
||||
{ |
|
||||
Vec<float,SIZE> retval(0.0f); |
|
||||
if (step >= steps) |
|
||||
{ |
|
||||
return retval; |
|
||||
} |
|
||||
|
|
||||
if (nodes != NULL && movementType == MovementCartBased) |
|
||||
{ |
|
||||
retval = nodes[step].acceleration; |
|
||||
} |
|
||||
|
|
||||
return retval; |
|
||||
} |
|
||||
@ -1,30 +1,60 @@ |
|||||
#ifndef _LINPOLYTRAJECTORY_H_ |
|
||||
#define _LINPOLYTRAJECTORY_H_ |
|
||||
|
#ifndef _LINEAR_TRAJECTORY_H_ |
||||
|
#define _LINEAR_TRAJECTORY_H_ |
||||
|
|
||||
|
#include "vec.h" |
||||
#include "Trajectroy.h" |
#include "Trajectroy.h" |
||||
|
#include "stdlib.h" |
||||
|
|
||||
template <unsigned SIZE> |
template <unsigned SIZE> |
||||
class LinearJointTrajectory: public Trajectory<SIZE> |
|
||||
|
class LinearJointTrajectory : public Trajectory<SIZE> |
||||
{ |
{ |
||||
public: |
public: |
||||
|
|
||||
LinearJointTrajectory( |
LinearJointTrajectory( |
||||
unsigned int steps_, |
|
||||
float totalTime_, |
|
||||
Vec<float,SIZE> jointMovement_); |
|
||||
LinearJointTrajectory( |
|
||||
unsigned int steps_, |
|
||||
float totalTime_, |
|
||||
Vec<float,SIZE> jointMovement_, |
|
||||
Vec<float,SIZE> velocity_); |
|
||||
~LinearJointTrajectory(); |
|
||||
|
float sampleTimeMs, |
||||
|
Vec<float,SIZE> maxJointVelocity, |
||||
|
Vec<float,SIZE> maxJointAcceleration, |
||||
|
Vec<float,SIZE> jointStart, |
||||
|
Vec<float,SIZE> jointEnd |
||||
|
) |
||||
|
{ |
||||
|
// calculate maximum velocity and acceleration |
||||
|
Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTimeMs; |
||||
|
Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs; |
||||
|
|
||||
|
// calculate delta movement |
||||
|
Vec<float,SIZE> jointMovement = jointEnd - jointStart; |
||||
|
Vec<float,SIZE> jointMovementAbs = jointMovement.abs(); |
||||
|
|
||||
|
// calculate sample count |
||||
|
|
||||
|
// calculate number of movement steps |
||||
|
Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity); |
||||
|
this->steps = ceil(minStepsPerJoint.max()); |
||||
|
|
||||
|
this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) malloc(sizeof(struct Trajectory<SIZE>::trajectoryNode)*this->steps); |
||||
|
|
||||
|
Vec<float,SIZE> jointLast = jointStart; |
||||
|
for( int i = 0 ; i < this->steps; ++i) |
||||
|
{ |
||||
|
jointLast = jointLast + jointMovement.celldivide((float) this->steps ); |
||||
|
this->nodes[0].jointPos = jointLast; |
||||
|
this->nodes[0].velocity = maxJointLocalVelocity; |
||||
|
this->nodes[0].acceleration; |
||||
|
} |
||||
|
} |
||||
|
~LinearJointTrajectory() |
||||
|
{ |
||||
|
free (this->nodes); |
||||
|
} |
||||
private: |
private: |
||||
|
|
||||
protected: |
protected: |
||||
}; |
}; |
||||
|
|
||||
template <unsigned SIZE> |
template <unsigned SIZE> |
||||
class LinearCartTrajectory: public Trajectory<SIZE> |
|
||||
|
class LinearCartTrajectory : public Trajectory<SIZE> |
||||
{ |
{ |
||||
}; |
|
||||
|
|
||||
|
|
||||
|
}; |
||||
#endif |
#endif |
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue