phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

fix lspb trajectory

master
philipp schoenberger 10 years ago
parent
2880d130e8
commit
6fb7300a9f
8 changed files with 79 additions and 55 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      lwrserv/include/BangBangTrajectory.h
  2. 90
      lwrserv/include/LSPBTrajectory.h
  3. 21
      lwrserv/include/Trajectroy.h
  4. 2
      lwrserv/include/mat.h
  5. 1
      lwrserv/include/robot.h
  6. 2
      lwrserv/program.cpp
  7. 5
      one_10_degree
  8. 4
      one_10_degree_lspb

9
lwrserv/include/BangBangTrajectory.h
View File

@ -21,7 +21,6 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
// 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;
@ -36,13 +35,12 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
// s = a * t^2 / 2
Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
//TODO check if mintime is neceesary
Vec<float,SIZE> time = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime;
std::cout << time << "time \n ";
std::cout << minBangBangTime << "minBangBangTime \n ";
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();
std::cout << minStepsPerJoint << "minStepsPerJoint \n";
this->steps = minStepsPerJoint.max();
if (this->steps == 0)
@ -78,7 +76,6 @@ class BangBangJointTrajectory : public Trajectory<SIZE>
this->nodes[i].jointPos = jointLast + this->nodes[i].velocity;
std::cout << count << this->nodes[i].velocity << this->nodes[i].jointPos <<"\n";
jointLast = this->nodes[i].jointPos;
velocityLast = this->nodes[i].velocity;
}

90
lwrserv/include/LSPBTrajectory.h
View File

@ -16,7 +16,6 @@ public:
Vec<float,SIZE> jointEnd
)
{
std::cout << "bang bang \n " ;
float MStoSec = 1000.0f;
float sampleTime = sampleTimeMs / MStoSec;
// calculate maximum velocity and acceleration
@ -35,19 +34,57 @@ public:
// 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);
Vec<float,SIZE> accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime;
Vec<float,SIZE> jointMovementRemaining = jointMovementAbs - maxJointVelocity.cellmultiply(maxJointVelocity).celldivide(maxJointAcceleration);
Vec<float,SIZE> maxVelocityPhaseThime = jointMovementRemaining.celldivide(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
Vec<float,SIZE> minStepsPerJoint = accelerationPhaseTime * 2.0f + maxVelocityPhaseThime;
std::cout << minStepsPerJoint << "minStepsPerJoint \n";
std::cout << accelerationPhaseTime << "a time \n";
std::cout << jointMovementRemaining << "reimain movement\n";
std::cout << maxVelocityPhaseThime << "max time \n";
minStepsPerJoint = minStepsPerJoint.ceil();
// 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;
Vec<float,SIZE> timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity);
std::cout << timeBangBang << "timeBangBang\n";
float steps_bangbang = timeBangBang.ceil().max();
// calculate maxAcceleration for bangbang
Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(steps_bangbang).celldivide(steps_bangbang)*2.0f;
std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n";
float percentLSPB = 0.7f;
Vec<float,SIZE> timeLSPB = timeBangBang * (1 + 0.25f * (1.0f - percentLSPB));
float timeBlend = ((timeBangBang /2.0f) * percentLSPB).ceil().max();
std::cout << timeBlend << "timeBlend \n";
std::cout << timeLSPB << "max time \n";
Vec<float,SIZE> minStepsPerJoint = timeLSPB.ceil();
this->steps = minStepsPerJoint.max();
if (this->steps == 0)
@ -58,32 +95,25 @@ public:
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 = (maxJointLocalAcceleration).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max();
Vec<float,SIZE> currMaxVelocity = (maxJointLocalVelocity ).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max();
std::cout << "max velo curr : " << currMaxVelocity << "\n";
int count = 0;
for( int i = 0 ; i < this->steps; ++i)
{
for (int joint = 0 ; joint < SIZE; ++joint)
if (i < timeBlend)
{
count += 1;
this->nodes[i].acceleration = currMaxAcceleration;
}else if (i < this->steps - timeBlend )
{
this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
}else
{
if (i < accelerationPhaseTime(i))
{
this->nodes[i].acceleration(i) = currMaxAcceleration(i);
}else if (i < accelerationPhaseTime(i) + jointMovementRemaining(i))
{
this->nodes[i].acceleration(i) = 0.0f;
}else
{
this->nodes[i].acceleration(i) = currMaxAcceleration(i)* -1.0f;
}
this->nodes[i].velocity(i) = velocityLast(i) + jointMovementSgn(i) * currMaxAcceleration(i);
count -= 1;
this->nodes[i].acceleration = currMaxAcceleration* -1.0f;
}
this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count;
this->nodes[i].jointPos = jointLast + this->nodes[i].velocity;
//std::cout << i << this->nodes[i].velocity << this->nodes[i].jointPos <<"\n";
jointLast = this->nodes[i].jointPos;
velocityLast = this->nodes[i].velocity;
}

21
lwrserv/include/Trajectroy.h
View File

@ -1,6 +1,7 @@
#ifndef _TRAJECTORY_H_
#define _TRAJECTORY_H_
#include "vec.h"
#include "mat.h"
#include <stdlib.h>
#include <iostream>
#include <fstream>
@ -48,8 +49,6 @@ static struct {
};
/// END OF TRAJECTORY LIST
static std::string toString(enum TrajectoryType type)
{
int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]);
@ -201,9 +200,9 @@ class Trajectory
return retval;
}
Mat<float,4> getNextCartPos ()
MatCarthesian getNextCartPos ()
{
Mat<float,4> retval(0.0f,1.0f);
MatCarthesian retval(0.0f,1.0f);
unsigned int pos = currentStep;
if (steps == 0)
return retval;
@ -233,7 +232,7 @@ class Trajectory
return retval;
}
if (nodes != NULL && movementType == MovementJointBased)
if (nodes != NULL)
{
retval = nodes[step].jointPos;
}
@ -241,9 +240,9 @@ class Trajectory
return retval;
}
Mat<float,4> getCartPos (unsigned int step)
MatCarthesian getCartPos (unsigned int step)
{
Vec<float,SIZE> retval(0.0f);
MatCarthesian retval(0.0f, 1.0f);
if (steps == 0)
return retval;
if (step >= steps)
@ -251,7 +250,7 @@ class Trajectory
return retval;
}
if (nodes != NULL && movementType == MovementJointBased)
if (nodes != NULL)
{
retval = nodes[step].cartPos;
}
@ -268,7 +267,7 @@ class Trajectory
return retval;
}
if (nodes != NULL && movementType == MovementCartBased)
if (nodes != NULL)
{
retval = nodes[step].velocity;
}
@ -285,7 +284,7 @@ class Trajectory
return retval;
}
if (nodes != NULL && movementType == MovementCartBased)
if (nodes != NULL)
{
retval = nodes[step].acceleration;
}
@ -296,7 +295,7 @@ class Trajectory
struct trajectoryNode
{
Vec<float,SIZE> jointPos;
Mat<float,4> cartPos;
MatCarthesian cartPos;
Vec<float,SIZE> velocity;
Vec<float,SIZE> acceleration;
};

2
lwrserv/include/mat.h
View File

@ -16,6 +16,8 @@ typedef Mat<double, 2> Mat2d;
typedef Mat<double, 3> Mat3d;
typedef Mat<double, 4> Mat4d;
typedef Mat<float, 4> MatCarthesian;
// template square matrix class for SIMPLE data types
template <class T, unsigned SIZE> class Mat

1
lwrserv/include/robot.h
View File

@ -5,7 +5,6 @@
#include "mat.h"
#include "friComm.h"
typedef Mat<float, 4> MatCarthesian;
typedef Vec<float, FRI_CART_VEC> VecTorque;
class Robot

2
lwrserv/program.cpp
View File

@ -206,7 +206,6 @@ void *threadRobotMovement(void *arg)
}
std::cout << "init position is " << currentJointPos << "\n";
while(true)
{
if (REAL_ROBOT)
@ -312,7 +311,6 @@ end:
static int trajcetorycount = 0;
trajcetorycount +=1;
currentTrajectory->saveToFile(std::string("trajectory/a.csv"));
// invalid trajectory skip it

5
one_10_degree
View File

@ -3,12 +3,11 @@
set timeout 1
spawn telnet localhost 8000
expect
set timeout 0.5
send "Hello Robot\n"
expect
send "SS 10\n"
send "SS 0.0110\n"
expect
send "ST JointBangBang\n"
expect
send "MPTPJ 0 0 0 0 0 0 100\n"
send "MPTPJ 0 0 0 0 0 0 10\n"
expect

4
one_10_degree_lspb
View File

@ -3,12 +3,12 @@
set timeout 1
spawn telnet localhost 8000
expect
set timeout 0.5
set timeout 1
send "Hello Robot\n"
expect
send "SS 10\n"
expect
send "ST JointLSPB\n"
expect
send "MPTPJ 0 0 0 0 0 0 100\n"
send "MPTPJ 0 0 0 0 0 0 10\n"
expect
Write Preview
Loading…
Cancel
Save