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.
60 lines
1.9 KiB
60 lines
1.9 KiB
#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>
|
|
{
|
|
public:
|
|
|
|
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>
|
|
{
|
|
|
|
};
|
|
#endif
|