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

new stuff

master
philipp schoenberger 10 years ago
parent
9496e6f4db
commit
cc471ebb8f
16 changed files with 862 additions and 448 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. 101
      lwrserv/.ycm_extra_conf.py
  2. 7
      lwrserv/BangBangTrajectroy.cpp
  3. 52
      lwrserv/LinearTrajectory.cpp
  4. 7
      lwrserv/Makefile
  5. 329
      lwrserv/SvrData.cpp
  6. 104
      lwrserv/SvrHandling.cpp
  7. 5
      lwrserv/SvrHandling.h
  8. 168
      lwrserv/Trajectroy.cpp
  9. 5
      lwrserv/global.cpp
  10. 8
      lwrserv/include/BangBangTrajectroy.h
  11. 14
      lwrserv/include/LinearTrajectory.h
  12. 98
      lwrserv/include/SvrData.h
  13. 61
      lwrserv/include/Trajectroy.h
  14. 59
      lwrserv/include/mat.h
  15. 2
      lwrserv/include/vec.h
  16. 270
      lwrserv/program.cpp

101
lwrserv/.ycm_extra_conf.py
View File

@ -0,0 +1,101 @@
# Partially stolen from https://bitbucket.org/mblum/libgp/src/2537ea7329ef/.ycm_extra_conf.py
import os
import ycm_core
# These are the compilation flags that will be used in case there's no
# compilation database set (by default, one is not set).
# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
flags = [
'-Wall',
'-Wextra',
'-Werror',
'-Wc++98-compat',
'-Wno-long-long',
'-Wno-variadic-macros',
'-fexceptions',
# THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
# language to use when compiling headers. So it will guess. Badly. So C++
# headers will be compiled as C headers. You don't want that so ALWAYS specify
# a "-std=<something>".
# For a C project, you would set this to something like 'c99' instead of
# 'c++11'.
'-std=c++11',
# ...and the same thing goes for the magic -x option which specifies the
# language that the files to be compiled are written in. This is mostly
# relevant for c++ headers.
# For a C project, you would set this to 'c' instead of 'c++'.
'-x', 'c++',
# This path will only work on OS X, but extra paths that don't exist are not
# harmful
'-isystem', '/System/Library/Frameworks/Python.framework/Headers',
'-isystem', '/usr/include',
'-isystem', '/usr/share/qt4/include',
'-isystem', '/usr/local/include',
'-I', 'include',
'-I', './include',
'-I', '.'
]
# Set this to the absolute path to the folder (NOT the file!) containing the
# compile_commands.json file to use that instead of 'flags'. See here for
# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
#
# Most projects will NOT need to set this to anything; you can just change the
# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
compilation_database_folder = ''
if compilation_database_folder:
database = ycm_core.CompilationDatabase( compilation_database_folder )
else:
database = None
def DirectoryOfThisScript():
return os.path.dirname( os.path.abspath( __file__ ) )
def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
if not working_directory:
return list( flags )
new_flags = []
make_next_absolute = False
path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
for flag in flags:
new_flag = flag
if make_next_absolute:
make_next_absolute = False
if not flag.startswith( '/' ):
new_flag = os.path.join( working_directory, flag )
for path_flag in path_flags:
if flag == path_flag:
make_next_absolute = True
break
if flag.startswith( path_flag ):
path = flag[ len( path_flag ): ]
new_flag = path_flag + os.path.join( working_directory, path )
break
if new_flag:
new_flags.append( new_flag )
return new_flags
def FlagsForFile( filename ):
if database:
# Bear in mind that compilation_info.compiler_flags_ does NOT return a
# python list, but a "list-like" StringVec object
compilation_info = database.GetCompilationInfoForFile( filename )
final_flags = MakeRelativePathsInFlagsAbsolute(
compilation_info.compiler_flags_,
compilation_info.compiler_working_dir_ )
else:
relative_to = DirectoryOfThisScript()
final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
return {
'flags': final_flags,
'do_cache': True
}

7
lwrserv/BangBangTrajectroy.cpp
View File

@ -5,14 +5,15 @@
#include "vec.h"
#include "SvrData.h"
template <class T, unsigned SIZE>
BangBangJointTrajectroy::BangBangJointTrajectroy(
template <unsigned SIZE>
BangBangJointTrajectroy<SIZE>::BangBangJointTrajectroy(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement
)
{
Vec< float, SIZE> velocity = jointMovement / steps * 1.5f;
Vec< float, SIZE> velocity = jointMovement / steps_ * 1.5f;
//TODO
LinearJointTrajectory(
steps_,
totalTime_,

52
lwrserv/LinearTrajectory.cpp
View File

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

7
lwrserv/Makefile
View File

@ -6,14 +6,14 @@
# autor Philipp Schoenberger #
# ph.schoenberger@googlemail.com #
################################################################################
LIBS = pthread
LIBS = pthread boost_thread boost_system
TOP_DIR = $(PWD)
TEST_DIR = $(BUILD_DIR)/test
INCLUDE_DIR = -I . \
-I ./include
# comment this out if you want debug compile info
Q=@
#Q=@
ifeq ($(MAKECMDGOALS),test)
GCOV := --coverage
@ -117,6 +117,7 @@ $(BUILD_DIR)/%.d: $(BUILD_DIR) $(TEST_DIR) %.cpp
$(Q)echo " [DEP] $@"
$(Q)touch $@
$(Q)$(CC) -M $(filter-out -g -o0 -o1 -o2 ,$(CPPFLAGS)) $(filter %.cpp,$^) >> $@
#$(Q)echo -n "$@ $(patsubst %.d,%.o,$@): " >$@
#$(Q)echo -n "$@ $(patsubst %.d,%.o,$@): " >$@
-include $(DEPS)

329
lwrserv/SvrData.cpp
View File

@ -2,7 +2,11 @@
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include "Trajectroy.h"
#include "SvrData.h"
#include "friComm.h"
#include "friremote.h"
#include <boost/thread/thread.hpp>
bool SvrData::instanceFlag = false;
SvrData* SvrData::single = 0;
@ -18,19 +22,23 @@ SvrData::SvrData(void)
robot.currentVelocity = 20;
robot.currentAcceleration = 10;
robot.joints = LBR_MNJ;
for (unsigned int i = 0; i < robot.joints; ++i)
for (unsigned int i = 0; i < JOINT_NUMBER; ++i)
{
robot.max.velocity[i] = maxVelJnt[i];
robot.max.accelaration[i] = maxAccJnt[i];
robot.max.torque[i] = maxTrqJnt[i];
robot.max.range[i] = maxRangeJnt[i];
robot.max.velocity(i) = maxVelJnt[i];
robot.max.accelaration(i) = maxAccJnt[i];
robot.max.torque(i) = maxTrqJnt[i];
robot.max.range(i) = maxRangeJnt[i];
}
trajectory.sampleTimeMs = 5.0f;
trajectory.cancel = false;
// init mutex for database
if (pthread_mutex_init(&dataLock, NULL) != 0)
printf("mutex init failed\n");
this->friInst = new friRemote;
}
SvrData::~SvrData()
@ -48,57 +56,83 @@ SvrData* SvrData::getInstance ()
return single;
}
void SvrData::lock()
friRemote* SvrData::getFriRemote()
{
friRemote* retval = NULL;
pthread_mutex_lock(&dataLock);
retval = this->friInst;
pthread_mutex_unlock(&dataLock);
return retval;
}
void SvrData::unlock()
void SvrData::updateMessurementData(
VecJoint newJointPos,
MatCarthesian newCartPos,
VecTorque newForceAndTorque
)
{
pthread_mutex_lock(&dataLock);
this->messured.jointPos = newJointPos;
this->messured.cartPos = newCartPos;
this->messured.forceAndTorque = newForceAndTorque;
pthread_mutex_unlock(&dataLock);
}
int SvrData::getMessuredJointPos(float* data, size_t size)
void SvrData::updateMessurement()
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(messured.jointPos))
return -EINVAL;
VecJoint newJointPos(0.0f);
MatCarthesian newCartPos(0.0f,1.0f);
VecTorque newForceAndTorque(0.0f);
/// do the update using fri
/// update current joint positions
VecJoint newJointPosComanded(friInst->getMsrCmdJntPosition());
VecJoint newJointPosOffset(friInst->getMsrCmdJntPositionOffset());
newJointPos = newJointPosComanded + newJointPosOffset;
/// update current cart position
newCartPos = friInst->getMsrCartPosition();
/// update current force and torque
newForceAndTorque = friInst->getFTTcp();
updateMessurementData(newJointPos,newCartPos,newForceAndTorque);
}
void SvrData::lock()
{
pthread_mutex_lock(&dataLock);
memcpy(data,messured.jointPos,size);
}
void SvrData::unlock()
{
pthread_mutex_unlock(&dataLock);
return 0;
}
Vec<float,LBR_MNJ> SvrData::getMessuredJointPos()
VecJoint SvrData::getMessuredJointPos()
{
Vec<float,LBR_MNJ> buf ;
VecJoint buf;
pthread_mutex_lock(&dataLock);
buf = messured.jointPos;
pthread_mutex_unlock(&dataLock);
return buf;
}
int SvrData::getMessuredCartPos (float* data, size_t size)
MatCarthesian SvrData::getMessuredCartPos()
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(messured.cartPos))
return -EINVAL;
MatCarthesian buf;
pthread_mutex_lock(&dataLock);
memcpy(data,messured.cartPos,size);
buf = messured.cartPos;
pthread_mutex_unlock(&dataLock);
return 0 ;
return buf;
}
Mat<float,LBR_MNJ> SvrData::getMessuredCartPos()
VecTorque SvrData::getMessuredForceTorque()
{
Mat<float,LBR_MNJ> buf ;
VecTorque buf;
pthread_mutex_lock(&dataLock);
buf = messured.cartPos;
buf = messured.forceAndTorque;
pthread_mutex_unlock(&dataLock);
return buf;
}
@ -116,226 +150,225 @@ int SvrData::getMessuredJacobian(float* data, size_t size)
return 0;
}
int SvrData::getMessuredForceTorque(float* data, size_t size)
VecJoint SvrData::getCommandedJointPos()
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(messured.forceAndTorque))
return -EINVAL;
VecJoint buff;
pthread_mutex_lock(&dataLock);
memcpy(data,messured.forceAndTorque,size);
buff = commanded.jointPos;
pthread_mutex_unlock(&dataLock);
return 0;
return buff;
}
int SvrData::getCommandedJointPos(float* data, size_t size)
void SvrData::setCommandedJointPos(VecJoint newJointPos)
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(commanded.jointPos))
return -EINVAL;
pthread_mutex_lock(&dataLock);
memcpy(data,commanded.jointPos,size);
commanded.jointPos = newJointPos;
pthread_mutex_unlock(&dataLock);
return 0;
}
Vec<float, LBR_MNJ> SvrData::getCommandedJointPos()
MatCarthesian SvrData::getCommandedCartPos ()
{
Vec<float, LBR_MNJ> buff;
MatCarthesian buff;
pthread_mutex_lock(&dataLock);
buff = commanded.jointPos;
buff = commanded.cartPos;
pthread_mutex_unlock(&dataLock);
return buff;
}
int SvrData::getCommandedCartPos (float* data, size_t size)
void SvrData::setCommandedCartPos(MatCarthesian newCartPos)
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(commanded.cartPos))
return -EINVAL;
pthread_mutex_lock(&dataLock);
memcpy(data,commanded.cartPos,size);
commanded.cartPos = newCartPos;
pthread_mutex_unlock(&dataLock);
return 0;
}
Mat<float, LBR_MNJ> SvrData::getCommandedCartPos ()
VecJoint SvrData::getMaxTorque()
{
Mat<float, LBR_MNJ> buff;
VecJoint buff;
pthread_mutex_lock(&dataLock);
buff = commanded.cartPos;
buff = robot.max.torque;
pthread_mutex_unlock(&dataLock);
return buff;
}
float SvrData::getMaxTorque(unsigned int pos)
VecJoint SvrData::getMaxAcceleration()
{
float retval = -1.0f;
if (pos >= robot.joints)
return retval;
VecJoint buff;
pthread_mutex_lock(&dataLock);
retval = robot.max.torque[pos-1];
buff = robot.max.accelaration;
pthread_mutex_unlock(&dataLock);
return retval;
return buff;
}
int SvrData::getMaxTorque(float* data, size_t size)
VecJoint SvrData::getMaxVelocity()
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(float)* robot.joints)
return -EINVAL;
VecJoint buff;
pthread_mutex_lock(&dataLock);
memcpy(data,robot.max.torque,size);
buff = robot.max.velocity;
pthread_mutex_unlock(&dataLock);
return 0;
return buff;
}
float SvrData::getMaxAcceleration(unsigned int pos)
VecJoint SvrData::getMaxRange()
{
float retval = -1.0f;
if (pos >= robot.joints)
return retval;
VecJoint buff;
pthread_mutex_lock(&dataLock);
retval = robot.max.accelaration[pos-1];
buff = robot.max.range;
pthread_mutex_unlock(&dataLock);
return retval;
return buff;
}
int SvrData::getMaxAcceleration(float* data, size_t size)
bool SvrData::checkJointRange(VecJoint vectorToCheck)
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(float)* robot.joints)
return -EINVAL;
bool retval = false;
pthread_mutex_lock(&dataLock);
memcpy(data,robot.max.accelaration,size);
Vec<float,LBR_MNJ> maxJointRange;
maxJointRange = robot.max.range;
pthread_mutex_unlock(&dataLock);
return 0;
for (int i = 0; i<JOINT_NUMBER; i++)
{
if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
{
// join angle is too big
return false;
}
}
// no join angle is too big
return false;
}
Vec<float,LBR_MNJ> SvrData::getMaxAcceleration()
int SvrData::setRobotVelocity(float newVelocity)
{
Vec<float,LBR_MNJ> buff;
unsigned int retval = 0;
pthread_mutex_lock(&dataLock);
buff = robot.max.accelaration;
robot.currentVelocity = newVelocity;
pthread_mutex_unlock(&dataLock);
return buff;
return retval;
}
float SvrData::getMaxVelocity(unsigned int pos)
float SvrData::getRobotVelocity()
{
float retval = -1.0f;
if (pos >= robot.joints)
return retval;
float retval = 0;
pthread_mutex_lock(&dataLock);
retval = robot.max.velocity[pos-1];
retval = robot.currentVelocity;
pthread_mutex_unlock(&dataLock);
return retval;
}
int SvrData::getMaxVelocity(float* data, size_t size)
int SvrData::setRobotAcceleration(float newAcceleration)
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(float)* robot.joints)
return -EINVAL;
unsigned int retval = 0;
pthread_mutex_lock(&dataLock);
memcpy(data,robot.max.velocity,size);
robot.currentAcceleration = newAcceleration;
pthread_mutex_unlock(&dataLock);
return 0;
return retval;
}
Vec<float,LBR_MNJ> SvrData::getMaxVelocity()
float SvrData::getRobotAcceleration()
{
Vec<float,LBR_MNJ> buff;
float retval = 0;
pthread_mutex_lock(&dataLock);
buff = robot.max.velocity;
retval = robot.currentAcceleration;
pthread_mutex_unlock(&dataLock);
return buff;
return retval;
}
float SvrData::getMaxRange(unsigned int pos)
unsigned int SvrData::getJoints()
{
float retval = -1.0f;
if (pos >= robot.joints)
return retval;
unsigned int retval = 0;
pthread_mutex_lock(&dataLock);
retval = robot.max.range[pos-1];
retval = JOINT_NUMBER;
pthread_mutex_unlock(&dataLock);
return retval;
}
int SvrData::getMaxRange(float* data, size_t size)
/// Trajectory function
float SvrData::getSampleTimeMs()
{
if (data == NULL)
return -EINVAL;
if (size != sizeof(float)* robot.joints)
return -EINVAL;
float retval = 0;
pthread_mutex_lock(&dataLock);
memcpy(data,robot.max.range,size);
retval = trajectory.sampleTimeMs;
pthread_mutex_unlock(&dataLock);
return 0;
return retval;
}
Vec<float, LBR_MNJ> SvrData::getMaxRange()
int SvrData::setSampleTimeMs(float newSampleTimeMs)
{
Vec<float, LBR_MNJ> retval ;
int retval = 0;
// sample time can not be negative or 0
if (newSampleTimeMs <= 0.0f)
return -1;
pthread_mutex_lock(&dataLock);
retval = robot.max.range;
trajectory.sampleTimeMs = newSampleTimeMs;
pthread_mutex_unlock(&dataLock);
return retval;
}
int SvrData::setRobotVelocity(float newVelocity)
class Trajectory<JOINT_NUMBER>* SvrData::popTrajectory()
{
unsigned int retval = 0;
class Trajectory<JOINT_NUMBER>* retval = NULL;
pthread_mutex_lock(&dataLock);
robot.currentVelocity = newVelocity;
retval = trajectory.list.front();
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotVelocity()
int SvrData::pushTrajectory(class Trajectory<JOINT_NUMBER>* newTrajectory)
{
float retval = 0;
if (newTrajectory == NULL)
return -1;
pthread_mutex_lock(&dataLock);
retval = robot.currentVelocity;
trajectory.list.push(newTrajectory);
pthread_mutex_unlock(&dataLock);
return retval;
return 0;
}
int SvrData::setRobotAcceleration(float newAcceleration)
bool SvrData::getTrajectoryCancel()
{
unsigned int retval = 0;
bool retval = false;
pthread_mutex_lock(&dataLock);
robot.currentAcceleration = newAcceleration;
retval = trajectory.cancel;
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotAcceleration()
bool SvrData::cancelCurrentTrajectory()
{
float retval = 0;
pthread_mutex_lock(&dataLock);
// set cancel state
trajectory.cancel = true;
// clear trajectory list
while(!trajectory.list.empty())
{
class Trajectory<JOINT_NUMBER>* currentTrajectory = trajectory.list.front();
if(currentTrajectory != NULL)
delete currentTrajectory;
trajectory.list.pop();
}
pthread_mutex_unlock(&dataLock);
// wait for stop signaled by calling cancel done
bool run = true;
while (run)
{
pthread_mutex_lock(&dataLock);
retval = robot.currentAcceleration;
if (trajectory.cancel == false)
{
run = false;
}
pthread_mutex_unlock(&dataLock);
return retval;
boost::this_thread::sleep( boost::posix_time::milliseconds(trajectory.sampleTimeMs) );
}
return true;
}
unsigned int SvrData::getJoints()
void SvrData::trajectoryCancelDone()
{
unsigned int retval = 0;
pthread_mutex_lock(&dataLock);
retval = robot.joints;
trajectory.cancel = false;
pthread_mutex_unlock(&dataLock);
return retval;
}

104
lwrserv/SvrHandling.cpp
View File

@ -9,6 +9,13 @@
#include "SvrHandling.h"
#include "SvrData.h"
#include "Trajectroy.h"
void printUsage(SocketObject& client)
{
client.Send(SVR_HELP_MSG);
}
void mattoabc(float M[3][4], float &a, float &b, float &c)
{
@ -265,21 +272,6 @@ double* kukaBackwardCalc(double M_[12], float arg[3])
return Joints;
}
bool checkJntRange(double Joints[7])
{
Vec<float,LBR_MNJ> maxJointRange = SvrData::getInstance()->getMaxRange();
for (int i = 0; i<LBR_MNJ; i++)
{
if (abs(Joints[i]) > abs(maxJointRange(i)))
{
// join angle is too big
return false;
}
}
// no join angle is too big
return false;
}
Mat4f vecToMat(float vec[12])
{
Mat4f result;
@ -445,6 +437,9 @@ void SvrHandling::handle(SocketObject& client)
} else if (cmd == "")
{
continue;
} else if (cmd == CMD_HELP)
{
printUsage(client);
} else if (cmd == CMD_QUIT || cmd == CMD_QUIT1 || cmd == CMD_QUIT2)
{
quit(client);
@ -485,14 +480,13 @@ void SvrHandling::GetPositionJoints(SocketObject& client)
string out = "";
stringstream ss (stringstream::in | stringstream::out);
float pos[LBR_MNJ];
if( 0 != SvrData::getInstance()->getMessuredJointPos(pos,sizeof(pos)))
client.Send("ERROR could not get joint pos");
VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
for (int i=0; i< LBR_MNJ; i++)
// disassemble command
for (int i=0; i< JOINT_NUMBER; i++)
{
ss.str("");
ss << (pos[i]*180/M_PI);
ss << (jointPos(i)*180/M_PI);
out += ss.str() + " ";
}
client.Send(out);
@ -503,14 +497,15 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client)
string out = "";
stringstream ss (stringstream::in | stringstream::out);
float pos[FRI_CART_FRM_DIM];
if( 0 != SvrData::getInstance()->getMessuredCartPos(pos,sizeof(pos)))
client.Send("ERROR could not get cart pos");
MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
for (int i=0; i< FRI_CART_FRM_DIM; i++)
{
int row = i / 4;
int column = i % 4;
ss.str("");
ss << (pos[i]);
ss << (cartPos(row,column));
out += ss.str() + " ";
}
client.Send(out);
@ -521,13 +516,11 @@ void SvrHandling::GetForceTorqueTcp(SocketObject& client)
string out = "";
stringstream ss (stringstream::in | stringstream::out);
float torque[FRI_CART_VEC];
if( 0 != SvrData::getInstance()->getMessuredForceTorque(torque,sizeof(torque)))
client.Send("ERROR could not get force and torque");
VecTorque torque = SvrData::getInstance()->getMessuredForceTorque();
for (int i=0; i< FRI_CART_VEC; i++)
{
ss.str("");
ss << torque[i];
ss << torque(i);
out += ss.str() + " ";
}
client.Send(out);
@ -535,44 +528,65 @@ void SvrHandling::GetForceTorqueTcp(SocketObject& client)
void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
{
__SVR_CURRENT_JOB = true;
//__SVR_CURRENT_JOB = true;
string buf;
float temp[LBR_MNJ];
stringstream ss(args);
stringstream ss2f;
vector<string> tokens;
// convert string to joint vector
VecJoint newJointPos(0.0f);
int i = 0;
while (ss >> buf)
{
if (i>LBR_MNJ-1)
// to many joint values
if (i>=JOINT_NUMBER)
{
client.Send(SVR_FALSE_RSP);
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return;
}
tokens.push_back(buf);
ss2f.flush();
ss2f.clear();
ss2f << buf;
ss2f >> temp[i];
ss2f >> newJointPos(i);
i++;
}
if (i<LBR_MNJ-1)
// to less joint values
if (i!=JOINT_NUMBER)
{
client.Send(SVR_FALSE_RSP);
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return ;
}
for (int i=0 ;i<LBR_MNJ; i++)
{
MSRCMDJNTPOS[i]=temp[i];
}
//Check for Joint Range
if (!checkJntRange(MSRCMDJNTPOS))
//check for joint range
if (!SvrData::getInstance()->checkJointRange(newJointPos))
{
string out = "Error: Joints out of Range!";
client.Send(out);
client.Send(SVR_OUT_OF_RANGE);
return;
}
//set new target for next trajectory
VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
class Trajectory<JOINT_NUMBER>* newTrajectory;
// calculate number of movement steps
dMaxSpeed = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration);
lMaxSpeed = dMaxSpeed.cellmultiply(dMaxSpeed).cellmultiply(maxJointLocalAcceleration) * 0.5f;
for (int j=0; j<JOINT_NUMBER; j++)
{
if (lMaxSpeed(j) > deltaAbs(j)/(double)2.0)
{
dGesamt(j) = sqrt(deltaAbs(j) / maxJointLocalAcceleration (j))*2.0;
} else
{
dGesamt(j) = dMaxSpeed(j)*2 + (deltaAbs(j)-lMaxSpeed(j)*2.0)/maxJointLocalVelocity(j);
}
}
int maxSteps = ceil(dGesamt.max());
newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(steps, totalTime, newJointPos);
__MSRCMDJNTPOS = true;
@ -915,7 +929,6 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
float temp[15];
stringstream ss(args);
stringstream ss2f;
vector<string> tokens;
int i = 0;
while (ss >> buf)
@ -925,7 +938,6 @@ void SvrHandling::MoveCartesian(SocketObject& client, string& args)
client.Send(SVR_FALSE_RSP);
return;
}
tokens.push_back(buf);
ss2f.flush();
ss2f.clear();
ss2f << buf;

5
lwrserv/SvrHandling.h
View File

@ -15,6 +15,10 @@
#define SVR_FAILED std::string("Failed.")
#define SVR_TRUE_RSP "true"
#define SVR_FALSE_RSP "false"
// server error messages
#define SVR_INVALID_ARGUMENT_COUNT "The argument count is not correct."
#define SVR_OUT_OF_RANGE "At least one argument is out of Range."
#define SVR_HELP_MSG "GPJ - GetPositionJoints \n"
//Begin SVR_COMMANDS
#define CMD_GetPositionJoints "GPJ"
@ -31,6 +35,7 @@
#define CMD_QUIT "Quit"
#define CMD_QUIT1 "quit"
#define CMD_QUIT2 "exit"
#define CMD_HELP "help"
#define CMD_ISKUKA "IsKukaLWR"
#define CMD_MoveCartesian "MC"
//End SVR_COMMANDS

168
lwrserv/Trajectroy.cpp
View File

@ -2,15 +2,18 @@
#include "Trajectroy.h"
#include "vec.h"
template <class T, unsigned SIZE>
Trajectory::Trajectory(
template <unsigned SIZE>
Trajectory<SIZE>::Trajectory(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement
)
{
movementType = MovementJointBased;
steps = steps_;
joints = SIZE;
currentStep = 0;
totalTime = totalTime_;
nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
@ -19,7 +22,164 @@ Trajectory::Trajectory(
(void) jointMovement;
}
Trajectory::~Trajectory()
template <unsigned SIZE>
Trajectory<SIZE>::Trajectory(
unsigned int steps_,
float totalTime_,
Mat<float,4> cartStart,
Mat<float,4> cartEnd
)
{
movementType = MovementCartBased;
steps = steps_;
currentStep = 0;
totalTime = totalTime_;
nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
// unused
(void) cartStart;
(void) cartEnd;
}
template <unsigned SIZE>
Trajectory<SIZE>::~Trajectory()
{
free(nodes);
}
template <unsigned SIZE>
unsigned int Trajectory<SIZE>::getSteps()
{
return steps;
}
template <unsigned SIZE>
unsigned int Trajectory<SIZE>::getRemainingSteps()
{
if (steps >= currentStep )
return steps - currentStep;
else
return 0;
}
template <unsigned SIZE>
unsigned int Trajectory<SIZE>::getCurrentStep()
{
return currentStep;
}
template <unsigned SIZE>
enum MovementType Trajectory<SIZE>::getMovementType()
{
return movementType;
}
template <unsigned SIZE>
Vec<float,SIZE> Trajectory<SIZE>::getNextJointPos ()
{
Vec<float,SIZE> retval(0.0f);
unsigned int pos = currentStep;
if (pos >= steps)
{
pos = steps;
}
if (nodes != NULL && movementType == MovementJointBased)
{
retval = nodes[pos].jointPos;
}
// increment step
currentStep ++;
return retval;
}
template <unsigned SIZE>
Mat<float,4> Trajectory<SIZE>::getNextCartPos ()
{
Mat<float,4> retval(0.0f,1.0f);
unsigned int pos = currentStep;
if (pos >= steps)
{
pos = steps;
}
if (nodes != NULL && movementType == MovementCartBased)
{
retval = nodes[pos].cartPos;
}
// increment step
currentStep ++;
return retval;
}
template <unsigned SIZE>
Vec<float,SIZE> Trajectory<SIZE>::getJointPos (unsigned int step)
{
Vec<float,SIZE> retval(0.0f);
if (step >= steps)
{
return retval;
}
if (nodes != NULL && movementType == MovementJointBased)
{
retval = nodes[step].cartPos;
}
return retval;
}
template <unsigned SIZE>
Mat<float,4> Trajectory<SIZE>::getCartPos (unsigned int step)
{
Mat<float,4> retval(0.0f, 1.0f);
if (step >= steps)
{
return retval;
}
if (nodes != NULL && movementType == MovementCartBased)
{
retval = nodes[step].cartPos;
}
return retval;
}
template <unsigned SIZE>
Vec<float,SIZE> Trajectory<SIZE>::getJointVelocity (unsigned int step)
{
Vec<float,SIZE> retval(0.0f);
if (step >= steps)
{
return retval;
}
if (nodes != NULL && movementType == MovementCartBased)
{
retval = nodes[step].velocity;
}
return retval;
}
template <unsigned SIZE>
Vec<float,SIZE> Trajectory<SIZE>::getJointAcceleration(unsigned int step)
{
Vec<float,SIZE> retval(0.0f);
if (step >= steps)
{
return retval;
}
if (nodes != NULL && movementType == MovementCartBased)
{
retval = nodes[step].acceleration;
}
return retval;
}

5
lwrserv/global.cpp
View File

@ -15,6 +15,7 @@ bool __DOUS2 = false;
int globi = 0;
#if 0
float MSRMSRJNTPOS[7];
double MSRCMDJNTPOS[7];
double MSRMSRCARTPOS[12];
@ -22,14 +23,14 @@ float MSRCMDCARTPOS[12];
double MSRMSRJACOBIAN[42];
double MSRMSRFTTCP[6];
double MSRCMDPOSE[3][4];
float USTarget[12];
float USApproach[12];
double VELOCITY = 20;
double ACCELARATION = 10;
//double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0};
double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0};
double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0};
double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0};
#endif

8
lwrserv/include/BangBangTrajectroy.h
View File

@ -3,11 +3,10 @@
#include "Trajectroy.h"
#include "vec.h"
class Trajectory;
class BangBangJointTrajectroy : public Trajectory
template <unsigned SIZE>
class BangBangJointTrajectroy : public Trajectory<SIZE>
{
public:
template <class T, unsigned SIZE>
BangBangJointTrajectroy(
unsigned int steps_,
float totalTime_,
@ -19,7 +18,8 @@ class BangBangJointTrajectroy : public Trajectory
protected:
};
class BangBangCartTrajectory: public Trajectory
template <unsigned SIZE>
class BangBangCartTrajectory: public Trajectory<SIZE>
{
};

14
lwrserv/include/LinearTrajectory.h
View File

@ -2,27 +2,27 @@
#define _LINPOLYTRAJECTORY_H_
#include "Trajectroy.h"
class LinearJointTrajectory: public Trajectory
template <unsigned SIZE>
class LinearJointTrajectory: public Trajectory<SIZE>
{
public:
template <class T, unsigned SIZE>
LinearJointTrajectory(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement);
template <class T, unsigned SIZE>
Vec<float,SIZE> jointMovement_);
LinearJointTrajectory(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement,
Vec<float,SIZE> velocity);
Vec<float,SIZE> jointMovement_,
Vec<float,SIZE> velocity_);
~LinearJointTrajectory();
private:
protected:
};
class LinearCartTrajectory: public Trajectory
template <unsigned SIZE>
class LinearCartTrajectory: public Trajectory<SIZE>
{
};

98
lwrserv/include/SvrData.h
View File

@ -4,39 +4,52 @@
#include <new>
#include <iostream>
#include <pthread.h>
#include <queue>
#include "Singleton.h"
#include "Trajectroy.h"
#include "vec.h"
#include "friComm.h"
#include "friremote.h"
#define JOINT_NUMBER (LBR_MNJ)
typedef Vec<float, JOINT_NUMBER> VecJoint;
typedef Vec<float, FRI_CART_VEC> VecTorque;
typedef Mat<float, 4> MatCarthesian;
class SvrData: public Singleton
{
private:
struct {
float jointPos[LBR_MNJ];
float cartPos[FRI_CART_FRM_DIM];
float jacobian[FRI_CART_VEC * LBR_MNJ];
float forceAndTorque[FRI_CART_VEC];
VecJoint jointPos;
MatCarthesian cartPos;
VecTorque forceAndTorque;
float jacobian[FRI_CART_VEC * JOINT_NUMBER];
} messured;
struct {
float jointPos[LBR_MNJ];
float cartPos[FRI_CART_FRM_DIM];
float pose[3][4]; //TODO what is this for
VecJoint jointPos;
MatCarthesian cartPos;
} commanded;
struct {
struct {
float velocity[LBR_MNJ];
float accelaration[LBR_MNJ];
float torque[LBR_MNJ];
float range[LBR_MNJ];
VecJoint velocity;
VecJoint accelaration;
VecJoint torque;
VecJoint range;
} max;
unsigned int joints;
float currentVelocity;
float currentAcceleration;
} robot;
struct {
float sampleTimeMs;
bool cancel;
std::queue< Trajectory<JOINT_NUMBER>* > list;
}trajectory;
friRemote* friInst;
pthread_mutex_t dataLock;
/// private constructor, because the database is a singleton
@ -45,45 +58,44 @@ private:
static bool instanceFlag;
static SvrData* single;
void updateMessurementData(
VecJoint newJointPos,
MatCarthesian newCartPos,
VecTorque newForceAndTorque
);
public:
~SvrData(void);
static SvrData* getInstance();
friRemote* getFriRemote();
void updateMessurement();
void lock();
void unlock();
int getMessuredJointPos(float* data, size_t size);
Vec<float,LBR_MNJ> getMessuredJointPos();
int getMessuredCartPos (float* data, size_t size);
Mat<float,LBR_MNJ> getMessuredCartPos();
VecJoint getMeasuredJointPos();
MatCarthesian getMeasuredCartPos();
int getMessuredJacobian(float* data, size_t size);
VecTorque getMessuredForceTorque();
VecJoint getMessuredJointPos();
MatCarthesian getMessuredCartPos();
int getMessuredForceTorque(float* data, size_t size);
Vec<float,LBR_MNJ> getMessuredForceTorque();
int getCommandedJointPos(float* data, size_t size);
Vec<float,LBR_MNJ> getCommandedJointPos();
int getCommandedCartPos (float* data, size_t size);
Mat<float,LBR_MNJ> getCommandedCartPos();
VecJoint getCommandedJointPos();
void setCommandedJointPos(VecJoint newJointPos);
float getMaxTorque(unsigned int pos);
int getMaxTorque(float* data, size_t size);
Vec<float,LBR_MNJ> getMaxTorque();
MatCarthesian getCommandedCartPos();
void setCommandedCartPos(MatCarthesian newCartPos);
float getMaxVelocity(unsigned int pos);
int getMaxVelocity(float* data, size_t size);
Vec<float,LBR_MNJ> getMaxVelocity();
VecJoint getMaxTorque();
VecJoint getMaxVelocity();
VecJoint getMaxAcceleration();
VecJoint getMaxRange();
float getMaxAcceleration(unsigned int pos);
int getMaxAcceleration(float* data, size_t size);
Vec<float,LBR_MNJ> getMaxAcceleration();
float getMaxRange(unsigned int pos);
int getMaxRange(float* data, size_t size);
Vec<float,LBR_MNJ> getMaxRange();
bool checkJointRange(VecJoint vectorToCheck);
float getRobotVelocity();
int setRobotVelocity(float newVelocity);
@ -92,5 +104,15 @@ public:
int setRobotAcceleration(float newVelocity);
unsigned int getJoints();
// trajectory functions
float getSampleTimeMs();
int setSampleTimeMs(float newSampleTime);
bool getTrajectoryCancel();
class Trajectory<JOINT_NUMBER>* popTrajectory();
int pushTrajectory(class Trajectory<JOINT_NUMBER>* newFrajectory);
bool cancelCurrentTrajectory();
void trajectoryCancelDone();
};
#endif

61
lwrserv/include/Trajectroy.h
View File

@ -2,43 +2,74 @@
#define _TRAJECTORY_H_
#include "vec.h"
// all types of trajectories
enum TrajectoryType {
TrajectoryDefault = 0,
TrajectoryJointLinear = 0,
TrajectoryCartLinear,
TrajectoryJointBangBang,
TrajectoryCartBangBang,
TrajectoryJointFivePoly,
TrajectoryCartFivePoly,
TrajectoryJointLSPB,
TrajectoryCartLSPB
};
enum MovementType
{
MovementJointBased= 0,
MovementCartBased
};
template <unsigned SIZE>
class Trajectory
{
public:
template <class T, unsigned SIZE>
Trajectory (
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement
);
Trajectory(
unsigned int steps_,
float totalTime_,
Mat<float,4> cartStart,
Mat<float,4> cartEnd
);
~Trajectory();
unsigned int getSteps();
unsigned int getJoints();
unsigned int getRemainingSteps();
unsigned int getCurrentStep();
enum MovementType getMovementType();
/**
*
*/
template <class T, unsigned SIZE> Vec<float,SIZE> getJointPos (unsigned int step);
template <class T, unsigned SIZE> Vec<float,SIZE> getCartPos (unsigned int step);
template <class T, unsigned SIZE> Vec<float,SIZE> getVelocity (unsigned int step);
template <class T, unsigned SIZE> Vec<float,SIZE> getAcceleration(unsigned int step);
Vec<float,SIZE> getNextJointPos ();
Mat<float,4> getNextCartPos ();
Vec<float,SIZE> getJointPos (unsigned int step);
Mat<float,4> getCartPos (unsigned int step);
Vec<float,SIZE> getJointVelocity (unsigned int step);
Vec<float,SIZE> getJointAcceleration(unsigned int step);
struct trajectoryNode
{
float jointPos;
float cartPos;
float velocity;
float acceleration;
Vec<float,SIZE> jointPos;
Mat<float,4>* cartPos;
Vec<float,SIZE> velocity;
Vec<float,SIZE> acceleration;
};
protected:
static const unsigned int defaultSteps = 100;
unsigned int steps;
unsigned int currentStep;
float totalTime;
unsigned int joints;
struct trajectoryNode* nodes;
enum MovementType movementType;
};
#endif

59
lwrserv/include/mat.h
View File

@ -16,6 +16,7 @@ typedef Mat<double, 2> Mat2d;
typedef Mat<double, 3> Mat3d;
typedef Mat<double, 4> Mat4d;
// template square matrix class for SIMPLE data types
template <class T, unsigned SIZE> class Mat
{
@ -103,8 +104,8 @@ public:
Mat<T, SIZE> operator + (const T &scalar)
{
Mat<T, SIZE> buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
for (unsigned int i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
buf(i,j) += m_aatData[i][j] + scalar;
return buf;
}
@ -112,8 +113,8 @@ public:
Mat<T, SIZE> operator - (const T &scalar)
{
Mat<T, SIZE> buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
for (unsigned int i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
buf(i,j) += m_aatData[i][j] - scalar;
return buf;
}
@ -121,17 +122,17 @@ public:
Mat<T, SIZE> operator * (const T &scalar)
{
Mat<T, SIZE> buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
buf(i,j) += m_aatData[i][j] * scalar;
for (unsigned int row=0; row<SIZE; row++)
for (unsigned int column=0; column<SIZE; column++)
buf(row,column) += m_aatData[row][column] * scalar;
return buf;
}
Mat<T, SIZE> operator / (const T &scalar)
{
Mat<T, SIZE> buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
for (unsigned int i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
buf(i,j) += m_aatData[i][j] / scalar;
return buf;
}
@ -139,16 +140,16 @@ public:
Mat<T, SIZE> operator * (const Mat<T, SIZE> &mat)
{
Mat<T, SIZE> buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
for (unsigned int i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
for (unsigned int a=0; a<SIZE; a++) // a
buf(i,j) += m_aatData[i][a] * mat(a,j);
return buf;
}
Mat<T, SIZE> &operator = (const T aatData[SIZE*SIZE])
{
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
for (unsigned int i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
m_aatData[i][j] = aatData[j+(i*4)];
return (*this); // also an R-value in e.g.
@ -161,8 +162,8 @@ public:
Mat<T, SIZE> operator * (const Vec<T, SIZE> &vec)
{
Vec<T, SIZE> buf;
for (unsigned int j=0; j<SIZE; j++) // SPALTE j
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // column j
for (unsigned int i=0; i<SIZE; i++) // row i
buf(i) += m_aatData[i][j]*vec(j);
return buf;
}
@ -171,8 +172,8 @@ public:
Mat<T,SIZE> abs()
{
Mat<T,SIZE> buf;
for (unsigned int j=0; j<SIZE; j++) // SPALTE j
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // column j
for (unsigned int i=0; i<SIZE; i++) // row i
buf.m_aatData[i][j] = abs(m_aatData[i][j]);
return buf;
}
@ -186,7 +187,7 @@ public:
return m_aatData[0][0] * m_aatData[1][1] - m_aatData[1][0]*m_aatData[0][1];
for (unsigned int i=0; i<SIZE; i++) // SPALTE j
for (unsigned int i=0; i<SIZE; i++) // column j
{
T multBuff;
multBuff = m_aatData[0][i];
@ -206,9 +207,9 @@ public:
Mat<T,SIZE> norm()
{
T buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
buf += pow(abs(m_aatData[i][j]),2);
for (unsigned int row=0; row < SIZE; row++)
for (unsigned int column=0; column < SIZE; column++)
buf += pow(abs(m_aatData[row][column]),2);
sqrt(buf);
return buf;
}
@ -216,8 +217,8 @@ public:
Mat<T,SIZE> transpose()
{
Mat<T, SIZE> buf;
for (unsigned int i=0; i<SIZE; i++) // ZEILE i
for (unsigned int j=0; j<SIZE; j++) // Spalte j
for (unsigned int i=0; i<SIZE; i++) // row i
for (unsigned int j=0; j<SIZE; j++) // column j
buf.m_aatData[i][j] = m_aatData[j][i];
return buf;
}
@ -235,6 +236,18 @@ public:
return thisTransposed;
}
void getData (T atData[SIZE*SIZE])
{
for (unsigned int row=0; row<SIZE; row++)
for (unsigned int column=0; column<SIZE; column++)
atData[row+column*SIZE] = m_aatData[row][column];
}
void setData (const T atData[SIZE*SIZE])
{
for (unsigned int row=0; row<SIZE; row++)
for (unsigned int column=0; column<SIZE; column++)
m_aatData[row][column] = atData[row+column*SIZE];
}
private:
T m_aatData[SIZE][SIZE];

2
lwrserv/include/vec.h
View File

@ -55,7 +55,7 @@ public:
m_atData[i] = atData[i];
}
void getData (const T atData[SIZE])
void getData (T atData[SIZE])
{
for (unsigned int i=0; i<SIZE; i++)
atData[i] = m_atData[i];

270
lwrserv/program.cpp
View File

@ -4,8 +4,10 @@
#include <error.h>
#include <errno.h>
#include <math.h>
#include <Trajectroy.h>
#include "SvrData.h"
#include <boost/thread/thread.hpp>
void printMat(Mat4f M)
{
@ -18,22 +20,22 @@ float* abctomat(float a, float b, float c)
float ca = cos(c);
float sa = sin(c);
rx(0,0) = 1;
rx(0,0) = 1.0f;
rx(1,1) = ca;
rx(1,2) = -sa;
rx(2,1) = sa;
rx(2,2) = ca;
rx(3,3) = 1;
rx(3,3) = 1.0f;
Mat4f ry;
float cb = cos(b);
float sb = sin(b);
ry(0,0) = cb;
ry(0,2) = sb;
ry(1,1) = 1;
ry(1,1) = 1.0f;
ry(2,0) = -sb;
ry(2,2) = cb;
ry(3,3) = 1;
ry(3,3) = 1.0f;
Mat4f rz;
float cc = cos(a);
@ -42,8 +44,8 @@ float* abctomat(float a, float b, float c)
rz(0,1) = -sc;
rz(1,0) = sc;
rz(1,1) = cc;
rz(2,2) = 1;
rz(3,3) = 1;
rz(2,2) = 1.0f;
rz(3,3) = 1.0f;
Mat4f result;
@ -96,78 +98,6 @@ float* mattoabc(float M[12])
return abc;
}
void doRalf()
{
// Global definitions
float lowStiff[6] = { 0.01f, 0.01f, 0.01f, 0.01f, 0.01f, 0.01f};
float highStiff[6] = {1000.0f, 1000.0f, 1000.0f, 10.0f, 10.0f, 10.0f};
float slowDamp[6] = { 0.007f, 0.007f, 0.007f, 0.007f, 0.007f, 0.007f};
float fastDamp[6] = { 0.7f, 0.7f, 0.7f, 0.7f, 0.7f, 0.7f};
float pos1[12] = {
1.0f, 0.0f, 0.0f, 500.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 500.0f
};
float pos2[12] = {
1.0f, 0.0f, 0.0f , 500.0f,
0.0f, 1.0f, 0.0f , 0.0f,
0.0f, 0.0f ,1.0f , 500.0f
};
friRemote friInst;
double timeCounter=0;
friInst.doDataExchange();
float* cartPos;
while(true)
{
// Time
timeCounter+=friInst.getSampleTime();
friInst.doReceiveData();
// Get all variables from FRI
float* jntPos = friInst.getMsrMsrJntPosition();
float* jacobian = friInst.getJacobian();
float* ftTcp = friInst.getFTTcp();
if (timeCounter < 20)
cartPos = friInst.getMsrCartPosition();
//friInst.doDataExchange();
//friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true);
#if 0
if (timeCounter>10 && timeCounter<10)
{
friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false);
} else
{
friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, false);
}
#endif
float npos[12];
memcpy(npos,cartPos,12);
friInst.doCartesianImpedanceControl(npos, highStiff, fastDamp, NULL, NULL, false);
#if 0
if (timeCounter<20)
{
friInst.doCartesianImpedanceControl(Test, highStiff, fastDamp, NULL, NULL, true);
} else if (timeCounter<30)
{
friInst.doCartesianImpedanceControl(cartPos, highStiff, fastDamp, NULL, NULL, true);
} else if (timeCounter<40)
{
friInst.doCartesianImpedanceControl(cartPos , highStiff, fastDamp, NULL, NULL, true);
} else
{
friInst.doDataExchange();
}
#endif
friInst.doSendData();
}
}
Mat4f vecToMat2(float vec[12])
{
Mat4f result;
@ -178,7 +108,7 @@ Mat4f vecToMat2(float vec[12])
result(i,j) = (float)vec[i*4+j];
}
}
result(3,3)=(float)1;
result(3,3)=1.0f;
return result;
}
@ -248,57 +178,161 @@ float* quattovec(float quat[4])
return vec;
}
void *threadFriDataExchange(void *arg)
void *threadRobotMovement(void *arg)
{
// unused
// unused parameters
(void) arg;
//doRalf();
Trajectory<JOINT_NUMBER>* currentTrajectory = NULL;
enum MovementType currentMovementType = MovementJointBased;
friRemote friInst;
VecJoint currentJointPos(0.0f);
MatCarthesian currentCartPos(0.0f,1.0f);
while (1)
while(1)
{
//#######################################
// Communication loop
// get current sample time cause it could be changed
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
friRemote* friInst = SvrData::getInstance()->getFriRemote();
// get current joint positions
for ( int i= 0; i < LBR_MNJ; i++)
// check if we have to cancel current trajectory
bool cancel = SvrData::getInstance()->getTrajectoryCancel();
if (cancel)
{
// free the current trajectory
if ( currentTrajectory != NULL)
{
MSRMSRJNTPOS[i] = friInst.getMsrCmdJntPosition()[i] + friInst.getMsrCmdJntPositionOffset()[i];
//MSRMSRJNTPOS[i] = friInst.getMsrMsrJntPosition()[i];
delete currentTrajectory;
currentTrajectory = NULL;
}
// signal waiting thread the cancellation
SvrData::getInstance()->trajectoryCancelDone();
goto end;
}
// get current jacobian
float* friJacobian = friInst.getJacobian();
if ( friJacobian == NULL)
// check for new trajectory
if (currentTrajectory == NULL)
{
fprintf(stderr,"Failed: could not get jacobian\n");
// get next trajectory from svrData
currentTrajectory = SvrData::getInstance()->popTrajectory();
// no new trajectory
if(currentTrajectory == NULL)
{
// stay on current position
goto end;
}
}
// get next commanded trajectory type
switch (currentTrajectory->getMovementType())
{
case MovementCartBased:
{
currentCartPos = currentTrajectory->getNextCartPos();
currentMovementType = MovementCartBased;
}
break;
case MovementJointBased:
{
currentJointPos = currentTrajectory->getNextJointPos();
currentMovementType = MovementJointBased;
}
for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++)
default:
{
MSRMSRJACOBIAN[i] = friJacobian[i];
// invalid trajectory skip it
delete currentTrajectory;
currentTrajectory = NULL;
goto end;
}
break;
}
// get current messured carthesian position
float* friMsrCartPosition = friInst.getMsrCartPosition();
if ( friMsrCartPosition == NULL)
end:
// TODO stop timer for last sent message
// time we used to come to this point caused by the calculation and lock time
// TODO calculate this time
float calculationTimeMs = 0.0f;
// wait only the remaining time
float waitingTimeMs = sampleTimeMs - calculationTimeMs;
// sleep the calculated waiting time
boost::this_thread::sleep( boost::posix_time::milliseconds(waitingTimeMs) );
switch (currentMovementType)
{
fprintf(stderr,"Failed: could not get Messurement in Carthesian Position\n");
case MovementJointBased:
{
// send the new joint values
float newJointPosToSend[JOINT_NUMBER] = {0.0f};
currentJointPos.getData(newJointPosToSend);
friInst->doPositionControl(newJointPosToSend);
}
break;
case MovementCartBased:
{
#if 0
float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0};
float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
float currentCartPosA[12] = {0.0f};
columncurrentCartPos.getData(newCartPosToSend);
friInst->doCartesianImpedanceControl(currentCartPosA,Stiff, Damp, NULL, NULL, true);
#endif
}
for ( int i =0; i < FRI_CART_FRM_DIM; i++)
break;
}
// start timer for the last sent msg
// TODO
// check if current trajectory is over
if(currentTrajectory != NULL && currentTrajectory->getRemainingSteps() == 0)
{
MSRMSRCARTPOS[i] = friMsrCartPosition[i];
// invalid trajectory skip it
delete currentTrajectory;
currentTrajectory = NULL;
}
}
}
#if 0
void *threadFriDataExchange(void *arg)
{
// unused
(void) arg;
friRemote* friInst = SvrData::getInstance()->getFriRemote();
while (1)
{
//#######################################
// Communication loop
// update current joint positions
SvrData::getInstance()->updateMessurement();
// get current joint position
VecJoint currentJointPos = SvrData::getInstance()->getMessuredJointPos();
MatCarthesian currentCartPost = SvrData::getInstance()->getMessuredCartPos();
// get current force and torque
float* friFTTcp = friInst.getFTTcp();
for ( int i = 0; i < FRI_CART_VEC; i++)
VecJoint currentForceAndTorque = SvrData::getInstance()->getMessuredForceTorque();
// get current jacobian
// TODO use svr data
float* friJacobian = friInst->getJacobian();
if ( friJacobian == NULL)
{
MSRMSRFTTCP[i] = friFTTcp[i];
fprintf(stderr,"Failed: could not get jacobian\n");
break;
}
for (int i = 0; i < FRI_CART_VEC*LBR_MNJ ; i++)
{
MSRMSRJACOBIAN[i] = friJacobian[i];
}
//#########################################################
@ -312,8 +346,7 @@ void *threadFriDataExchange(void *arg)
Vec<float,LBR_MNJ> dMaxSpeed;
Vec<float,LBR_MNJ> lMaxSpeed;
Vec<float,LBR_MNJ> dGesamt;
float sampleTime = 0.005f;
float sampleTime = SvrData::getInstance()->getSampleTimeMs();
// get current robot constraints
Vec<float,LBR_MNJ> maxVelocityJoint = SvrData::getInstance()->getMaxVelocity();
@ -364,13 +397,13 @@ void *threadFriDataExchange(void *arg)
maxJointLocalVelocity(j) = dMaxSpeed(j)*maxJointLocalAcceleration(j);
}
// do the trajectory
{
Vec<float,LBR_MNJ> currentInk;
Vec<float,LBR_MNJ> currentInkLast;
Vec<float,LBR_MNJ> currentDist;
Vec<float,LBR_MNJ> currentDistLast;
// bang bang
for (int i=0;i<maxSteps;i++)
{
@ -389,10 +422,10 @@ void *threadFriDataExchange(void *arg)
currentDist(j) = currentDistLast(j) + sgn(delta(j))*currentInk(j);
currentInkLast(j) = currentInk(j);
currentDistLast(j) = currentDist(j);
MSRMSRJNTPOS[j]+= sgn(delta(j))*currentInk(j)*(1./180*M_PI);
MSRMSRJNTPOS[j] += sgn(delta(j))*currentInk(j)*(1./180*M_PI);
}
// set new position
friInst.doPositionControl(MSRMSRJNTPOS);
friInst->doPositionControl(MSRMSRJNTPOS);
}
}
end:
@ -424,8 +457,8 @@ end:
{
for ( int i =0; i < FRI_CART_FRM_DIM; i++)
{
Pos[i] = friInst.getMsrCartPosition()[i];
MSRMSRCARTPOS[i] = friInst.getMsrCartPosition()[i];
Pos[i] = friInst->getMsrCartPosition()[i];
MSRMSRCARTPOS[i] = friInst->getMsrCartPosition()[i];
deltaCart[i] = MSRCMDCARTPOS[i]-Pos[i];
}
sum = 0;
@ -484,24 +517,25 @@ end:
{
Pos[i]+= deltaCart[i]*0.02;
}
friInst.doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true);
friInst->doCartesianImpedanceControl(Pos, Stiff,Damp,NULL,NULL, true);
}
}
//float Stiff[6] = {1000.0, 1000.0, 1000.0, 10.0, 10.0, 10.0};
//float Damp[6] = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7};
//float Test[12] = {MSRMSRCARTPOS[0],MSRMSRCARTPOS[1],MSRMSRCARTPOS[2],MSRMSRCARTPOS[3],MSRMSRCARTPOS[4],MSRMSRCARTPOS[5],MSRMSRCARTPOS[6],MSRMSRCARTPOS[7],MSRMSRCARTPOS[8],MSRMSRCARTPOS[9],MSRMSRCARTPOS[10],MSRMSRCARTPOS[11]};
//friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
//friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
// innerhlab 5ms
friInst.doPositionControl(MSRMSRJNTPOS);
friInst->doPositionControl(MSRMSRJNTPOS);
//i = 0;
//}
//friInst.doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
//friInst->doCartesianImpedanceControl(Test,Stiff, Damp, NULL, NULL, true);
}
return NULL;
}
#endif
int main()
{
@ -509,7 +543,7 @@ int main()
//Setting pthread for FRI interface
pthread_t fri_t;
//Start fri_thread
err = pthread_create(&fri_t,NULL,&threadFriDataExchange,NULL);
err = pthread_create(&fri_t,NULL,&threadRobotMovement,NULL);
if (err > 0 )
{
cerr << "Failed: could not create thread\n" << endl;

Write Preview
Loading…
Cancel
Save