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.
74 lines
2.7 KiB
74 lines
2.7 KiB
#include "LinearTrajectory.h"
|
|
#include "vec.h"
|
|
#include "stdlib.h"
|
|
|
|
template <unsigned SIZE>
|
|
LinearJointTrajectory<SIZE>::LinearJointTrajectory(
|
|
unsigned int steps_,
|
|
float totalTime_,
|
|
Vec<float,SIZE> jointMovement_
|
|
)
|
|
{
|
|
Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
|
|
LinearJointTrajectory(
|
|
steps_,
|
|
totalTime_,
|
|
jointMovement_,
|
|
velocity);
|
|
}
|
|
|
|
template <unsigned SIZE>
|
|
LinearJointTrajectory<SIZE>::LinearJointTrajectory(
|
|
unsigned int steps_,
|
|
float totalTime_,
|
|
Vec<float,SIZE> jointMovement,
|
|
Vec<float,SIZE> velocity
|
|
)
|
|
{
|
|
super(steps_, totalTime_, jointMovement);
|
|
|
|
if ( velocity > jointMovement/totalTime_)
|
|
{
|
|
std::cerr << "velocity is to small for Trajectory\n";
|
|
}
|
|
|
|
if ( velocity < (2.0f*jointMovement)/totalTime_)
|
|
{
|
|
std::cerr << "velocity is to big for Trajectory\n";
|
|
}
|
|
|
|
for (unsigned int currJoint = 0 ; currJoint < SIZE; currJoint++)
|
|
{
|
|
unsigned int timeBlend = (jointMovement(currJoint) + velocity(currJoint)*totalTime_)/ velocity(currJoint);
|
|
float acceleration = velocity(currJoint) / timeBlend;
|
|
|
|
for (unsigned int currStep = 0 ; currStep < steps_ ; ++currStep)
|
|
{
|
|
float currentTime = currStep * totalTime_ / steps_;
|
|
if (currentTime <= timeBlend)
|
|
{
|
|
// speed up till blend time is reached
|
|
this->nodes[currStep].jointPos = acceleration/2.0f * currentTime * currentTime;
|
|
this->nodes[currStep].velocity = acceleration * currentTime;
|
|
this->nodes[currStep].acceleration = acceleration;
|
|
} else
|
|
if ( currentTime <= totalTime_ - timeBlend)
|
|
{
|
|
// constant velocity
|
|
this->nodes[currStep].jointPos = (jointMovement(currJoint) - velocity(currJoint) * totalTime_)/2.0f + velocity(currJoint) * currentTime;
|
|
this->nodes[currStep].velocity = velocity(currJoint);
|
|
this->nodes[currStep].acceleration = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
// slow down until aim is reached
|
|
// speed up till blend time is reached
|
|
this->nodes[currStep].jointPos = jointMovement(currJoint) - acceleration/2*totalTime_*totalTime_ + acceleration * totalTime_ * currentTime - acceleration/2.0f * currentTime *currentTime;
|
|
this->nodes[currStep].velocity = acceleration*timeBlend - acceleration * currentTime ;
|
|
this->nodes[currStep].acceleration = -acceleration;
|
|
}
|
|
// calculate Cartesian positions
|
|
this->nodes[currStep].cartPos = 0;
|
|
}
|
|
}
|
|
}
|