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.
216 lines
8.2 KiB
216 lines
8.2 KiB
#ifndef _LSPBTRAJCETORY_H_
|
|
#define _LSPBTRAJCETORY_H_
|
|
#include <math.h>
|
|
#include "Trajectroy.h"
|
|
#include "sgn.h"
|
|
|
|
/**
|
|
* @addtogroup trajectory
|
|
* @{
|
|
* @addtogroup LSPB
|
|
* @{
|
|
* @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
|
|
* @version 1.0
|
|
*
|
|
* @section LICENSE
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License as
|
|
* published by the Free Software Foundation; either version 2 of
|
|
* the License, or (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* General Public License for more details at
|
|
* https://www.gnu.org/copyleft/gpl.html
|
|
*
|
|
* @section DESCRIPTION
|
|
*
|
|
* This file contains the Trajectory for a LSPB trajectory.
|
|
* The LSPB trajectory is a trajectory with linear acceleration
|
|
* phase followed by a constant speed and a de-acceleration phase
|
|
* The speed curve looks with this like a symmetric Trapez.
|
|
*
|
|
* The slowest joint is defining the speed of the other joints.
|
|
* By that all joints start and stop the movement synchronously
|
|
*/
|
|
|
|
/**
|
|
* Class for a Bang Bang Trajectory based on joint angles.
|
|
* The joint start and stop synchron
|
|
*
|
|
* This class is based on the Trajectory joint Template so it
|
|
* all possible joint counts
|
|
*
|
|
* @see Trajectory
|
|
*/
|
|
template <unsigned SIZE>
|
|
class LSPBJointTrajectory: public Trajectory<SIZE>
|
|
{
|
|
public:
|
|
/**
|
|
* standard constructor which will setup the class for a
|
|
* trajectory from start to end position with a synchron movement based on joints
|
|
*
|
|
* The trajectory will be of type LSPB (Linear Segment with Parabolic Blend)
|
|
*
|
|
* @param sampleTimeMs The discrete time interval for each step within the Trajectory with the unit [ milliseconds ]
|
|
* @param maxJointVelocity The maximum velocity for each joint with the unit [ degree/ seconds ]
|
|
* @param maxJointAcceleration The maximum acceleration for each joint with the unit [ degree/(seconds^2) ]
|
|
* @param jointStart The starting point for each joint in [ degree ]
|
|
* @param jointEnd The point which should be reached at the end of the Trajectory [ degree ]
|
|
*/
|
|
LSPBJointTrajectory(
|
|
float sampleTimeMs,
|
|
Vec<float,SIZE> maxJointVelocity,
|
|
Vec<float,SIZE> maxJointAcceleration,
|
|
Vec<float,SIZE> jointStart,
|
|
Vec<float,SIZE> jointEnd
|
|
)
|
|
{
|
|
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;
|
|
std::cout << maxJointLocalVelocity << ", " << maxJointLocalAcceleration << "\n";
|
|
|
|
// 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
|
|
Vec<float,SIZE> maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity);
|
|
maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity);
|
|
|
|
// v ^
|
|
// |
|
|
// v_bang| /\ <- bang bang trajectory
|
|
// | / \
|
|
// v_lspb| _____\___ <- lspb trajectory
|
|
// | / . .
|
|
// | / . . \
|
|
// | / . . \
|
|
// |/________.__.___\____> t
|
|
// | t1| t2 | t3|
|
|
// |tx|
|
|
//
|
|
// t1/t3 = acceleration phase
|
|
// t2 = constant speed phase
|
|
//
|
|
// t = t1 + t2 +t3
|
|
//
|
|
// v_lspb = percent * v_bang
|
|
//
|
|
// t = t_bang + t_x
|
|
// t = t_bang + (v_bang - v_lspb)/a * 2
|
|
//
|
|
// t = t_bang + (t_bang/2 * a - percent *t_bang/2*a) / a *2
|
|
// t = t_bang + (1- percent) * (t_bang/2 * a) / a *2
|
|
// t = t_bang + (1- percent) * t_bang
|
|
// t = (2-percent) * t_bang
|
|
|
|
// calculate time for bang bang
|
|
Vec<float,SIZE> accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime;
|
|
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;
|
|
|
|
// calculate the time and steps of the bangbang Trajectory
|
|
Vec<float,SIZE> timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity);
|
|
float steps_bangbang = timeBangBang.ceil().max();
|
|
|
|
// calculate maxAcceleration for a bangbang trajectory
|
|
Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(steps_bangbang).celldivide(steps_bangbang)*2.0f;
|
|
|
|
// use only a percentage of the bangbang max velocity
|
|
float percentLSPB = 0.7f;
|
|
Vec<float,SIZE> timeLSPB = timeBangBang * (1 + 0.25f * (1.0f - percentLSPB));
|
|
float timeBlend = ((timeBangBang /2.0f) * percentLSPB).ceil().max();
|
|
|
|
// round up and use the maximum of all possible steps
|
|
Vec<float,SIZE> minStepsPerJoint = timeLSPB.ceil();
|
|
this->steps = minStepsPerJoint.max();
|
|
if (this->steps == 0)
|
|
this->steps +=1;
|
|
|
|
// allocate an trajectory with this size
|
|
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);
|
|
|
|
|
|
// initialize the accelerated counts to zero
|
|
int count = 0;
|
|
|
|
for( int i = 0 ; i < this->steps; ++i)
|
|
{
|
|
if (i < timeBlend)
|
|
{
|
|
// acceleration phase
|
|
count += 1;
|
|
this->nodes[i].acceleration = currMaxAcceleration;
|
|
}else if (i < this->steps - timeBlend )
|
|
{
|
|
// constant speed phase
|
|
this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
|
|
}else
|
|
{
|
|
// deacceleration phase
|
|
count -= 1;
|
|
this->nodes[i].acceleration = currMaxAcceleration* -1.0f;
|
|
}
|
|
|
|
// calculate velocity and joint position accordingly to the acceleration
|
|
// @warning to not add floats over and over again or the error will be huge
|
|
this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count;
|
|
this->nodes[i].jointPos = jointLast + this->nodes[i].velocity;
|
|
|
|
// check if we already reached end
|
|
// This can happen if the steps calculation was
|
|
// to far off caused by floating point precision
|
|
|
|
float maxdiffLast = (jointEnd - jointLast).abs().max();
|
|
float maxdiffCurr = (jointEnd - this->nodes[i].jointPos).abs().max();
|
|
if ( maxdiffLast < maxdiffCurr)
|
|
{
|
|
// mark the current index as the last one within this trajectory
|
|
this->steps = i+1;
|
|
// set the end position
|
|
this->nodes[i].jointPos = jointEnd;
|
|
break;
|
|
}
|
|
|
|
jointLast = this->nodes[i].jointPos;
|
|
velocityLast = this->nodes[i].velocity;
|
|
}
|
|
}
|
|
};
|
|
|
|
/**
|
|
* Class for a LSPB Trajectory based on homogenous
|
|
* Cartesian Positions.
|
|
* The joint start and stop synchron
|
|
*
|
|
* This class is based on the Trajectory joint Template so it
|
|
* all possible joint counts
|
|
*
|
|
* @TODO implement the Cartesian movement constructor for LSPB trajectories
|
|
* @see Trajectory
|
|
*/
|
|
template <unsigned SIZE>
|
|
class LSPBCartTrajectory: public Trajectory<SIZE>
|
|
{
|
|
};
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|
|
#endif
|