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 "stdlib.h" |
|||
|
|||
template <unsigned SIZE> |
|||
class LinearJointTrajectory: public Trajectory<SIZE> |
|||
class LinearJointTrajectory : public Trajectory<SIZE> |
|||
{ |
|||
public: |
|||
|
|||
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: |
|||
|
|||
protected: |
|||
}; |
|||
|
|||
template <unsigned SIZE> |
|||
class LinearCartTrajectory: public Trajectory<SIZE> |
|||
class LinearCartTrajectory : public Trajectory<SIZE> |
|||
{ |
|||
}; |
|||
|
|||
|
|||
}; |
|||
#endif |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue