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

90
lwrserv/include/LSPBTrajectory.h
View File

@ -16,7 +16,6 @@ public:
Vec<float,SIZE> jointEnd Vec<float,SIZE> jointEnd
) )
{ {
std::cout << "bang bang \n " ;
float MStoSec = 1000.0f; float MStoSec = 1000.0f;
float sampleTime = sampleTimeMs / MStoSec; float sampleTime = sampleTimeMs / MStoSec;
// calculate maximum velocity and acceleration // calculate maximum velocity and acceleration
@ -35,19 +34,57 @@ public:
// one joint has to reach maxvelocity the others are stepped down to // one joint has to reach maxvelocity the others are stepped down to
// calculate time if acceleration is enouth to reach max speed // calculate time if acceleration is enouth to reach max speed
Vec<float,SIZE> maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity); Vec<float,SIZE> maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity);
maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity); 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(); this->steps = minStepsPerJoint.max();
if (this->steps == 0) if (this->steps == 0)
@ -58,32 +95,25 @@ public:
Vec<float,SIZE> jointLast = jointStart; Vec<float,SIZE> jointLast = jointStart;
Vec<float,SIZE> velocityLast(0.0f); 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 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; 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; jointLast = this->nodes[i].jointPos;
velocityLast = this->nodes[i].velocity; velocityLast = this->nodes[i].velocity;
} }

21
lwrserv/include/Trajectroy.h
View File

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

1
lwrserv/include/robot.h
View File

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

2
lwrserv/program.cpp
View File

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

5
one_10_degree
View File

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

4
one_10_degree_lspb
View File

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