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.
75 lines
2.9 KiB
75 lines
2.9 KiB
#ifndef _BANGBANGTRAJCETORY_H_
|
|
#define _BANGBANGTRAJCETORY_H_
|
|
#include "Trajectroy.h"
|
|
#include "vec.h"
|
|
|
|
template <unsigned SIZE>
|
|
class BangBangJointTrajectory : public Trajectory<SIZE>
|
|
{
|
|
public:
|
|
BangBangJointTrajectory(
|
|
float sampleTimeMs,
|
|
Vec<float,SIZE> maxJointVelocity,
|
|
Vec<float,SIZE> maxJointAcceleration,
|
|
Vec<float,SIZE> jointStart,
|
|
Vec<float,SIZE> jointEnd
|
|
)
|
|
{
|
|
float MStoSec = 1000.0f;
|
|
// calculate maximum velocity and acceleration
|
|
Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
|
|
Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
|
|
|
|
// calculate delta movement
|
|
Vec<float,SIZE> jointMovement = jointEnd - jointStart;
|
|
Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
|
|
|
|
// calculate sample count
|
|
|
|
// calculate number of movement steps
|
|
// one joint has to reach maxvelocity the others are stepped down to
|
|
Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
|
|
this->steps = ceil(minStepsPerJoint.max());
|
|
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;
|
|
float percentStep = 1.0f / ((this->steps+1)/2);
|
|
for( int i = 0 ; i < this->steps; ++i)
|
|
{
|
|
// acceleration phase
|
|
if (i > this->steps /2 )
|
|
{
|
|
float percent = i * percentStep;
|
|
this->nodes[i].velocity = maxJointLocalVelocity * percent;
|
|
this->nodes[i].acceleration = maxJointLocalVelocity * percentStep;
|
|
}
|
|
// deacceleration phase
|
|
else
|
|
{
|
|
float percent = ( i - this->steps/2 ) * percentStep;
|
|
this->nodes[i].velocity = maxJointLocalVelocity * percent;
|
|
this->nodes[i].acceleration = maxJointLocalVelocity * (-1.0 * percentStep);
|
|
}
|
|
|
|
jointLast = jointLast + this->nodes[i].velocity;
|
|
this->nodes[i].jointPos = jointLast;
|
|
}
|
|
// 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 BangBangCartTrajectory: public Trajectory<SIZE>
|
|
{
|
|
};
|
|
|
|
#endif
|