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.
93 lines
3.7 KiB
93 lines
3.7 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
|
|
)
|
|
{
|
|
std::cout << "bang bang \n " ;
|
|
float MStoSec = 1000.0f;
|
|
float sampleTime = sampleTimeMs / MStoSec;
|
|
// calculate maximum velocity and acceleration
|
|
Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTime;
|
|
Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTime;
|
|
|
|
// calculate delta movement
|
|
Vec<float,SIZE> jointMovement = jointEnd - jointStart;
|
|
Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
|
|
Vec<float,SIZE> jointMovementSgn = jointMovement.sgn();
|
|
|
|
// calculate sample count
|
|
|
|
// calculate number of movement steps
|
|
// one joint has to reach maxvelocity the others are stepped down to
|
|
// calculate time if acceleration is enouth to reach max speed
|
|
// s = a * t^2 / 2
|
|
Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
|
|
//TODO check if mintime is neceesary
|
|
Vec<float,SIZE> timeMaxAcceleration = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime;
|
|
Vec<float,SIZE> timeMaxVelocity = ((jointMovementAbs/2.0f).celldivide(maxJointVelocity) * 2.0f)*2.0f / sampleTime;
|
|
Vec<float,SIZE> time = timeMaxAcceleration.cellmax(timeMaxVelocity);
|
|
|
|
Vec<float,SIZE> minStepsPerJoint = time;//jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
|
|
minStepsPerJoint = minStepsPerJoint.ceil();
|
|
|
|
this->steps = 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;
|
|
Vec<float,SIZE> velocityLast(0.0f);
|
|
|
|
// percentage of max velocity
|
|
//Vec<float,SIZE> currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ;
|
|
// s = a* t^2 / 2 => a = s* 2 / t^2
|
|
Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f;
|
|
std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n";
|
|
|
|
float count = 0.0f;
|
|
for( int i = 0 ; i < this->steps; ++i)
|
|
{
|
|
// acceleration phase
|
|
if (i <= this->steps /2 )
|
|
{
|
|
this->nodes[i].acceleration = currMaxAcceleration ;
|
|
count +=1.0f;
|
|
}
|
|
// deacceleration phase
|
|
else
|
|
{
|
|
this->nodes[i].acceleration = currMaxAcceleration * -1.0f;
|
|
count -=1.0f;
|
|
}
|
|
this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count;
|
|
this->nodes[i].jointPos = jointLast + this->nodes[i].velocity;
|
|
|
|
|
|
jointLast = this->nodes[i].jointPos;
|
|
velocityLast = this->nodes[i].velocity;
|
|
}
|
|
}
|
|
private:
|
|
|
|
protected:
|
|
|
|
};
|
|
template <unsigned SIZE>
|
|
class BangBangCartTrajectory: public Trajectory<SIZE>
|
|
{
|
|
};
|
|
|
|
#endif
|