You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
69 lines
2.6 KiB
69 lines
2.6 KiB
#ifndef _LINEAR_TRAJECTORY_H_
|
|
#define _LINEAR_TRAJECTORY_H_
|
|
|
|
#include "Trajectroy.h"
|
|
#include "stdlib.h"
|
|
#include "Vec.h"
|
|
#include "Mat.h"
|
|
|
|
template <unsigned SIZE>
|
|
class LinearJointTrajectory : public Trajectory<SIZE>
|
|
{
|
|
public:
|
|
|
|
LinearJointTrajectory(
|
|
float sampleTimeMs,
|
|
Vec<float,SIZE> maxJointVelocity,
|
|
Vec<float,SIZE> maxJointAcceleration,
|
|
Vec<float,SIZE> jointStart,
|
|
Vec<float,SIZE> jointEnd
|
|
)
|
|
{
|
|
std::cout << "linear \n " ;
|
|
float MStoSec = 1000.0f;
|
|
// calculate maximum velocity and acceleration
|
|
Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
|
|
Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
|
|
std::cout << "max : " << maxJointLocalVelocity << "\n";
|
|
|
|
// 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);
|
|
std::cout << "minsteps : " << minStepsPerJoint << "\n";
|
|
std::cout << "steps : " << minStepsPerJoint.max() << "\n";
|
|
this->steps = ceil(minStepsPerJoint.max());
|
|
std::cout << "steps : " << this->steps << "\n";
|
|
if (this->steps == 0)
|
|
this->steps +=1;
|
|
|
|
this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(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[i].jointPos = jointLast;
|
|
this->nodes[i].velocity = maxJointLocalVelocity;
|
|
this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
|
|
}
|
|
// last step myth be to wide so cut it to the wanted position
|
|
this->nodes[this->steps-1].jointPos = jointEnd;
|
|
this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth
|
|
this->nodes[this->steps-1].acceleration = Vec<float,SIZE>(0.0f);
|
|
}
|
|
private:
|
|
|
|
protected:
|
|
};
|
|
|
|
template <unsigned SIZE>
|
|
class LinearCartTrajectory : public Trajectory<SIZE>
|
|
{
|
|
|
|
};
|
|
#endif
|