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

trivial

master
philipp schoenberger 10 years ago
parent
cc471ebb8f
commit
5c12d4de02
8 changed files with 315 additions and 285 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. 20
      lwrserv/LinearTrajectory.cpp
  2. 82
      lwrserv/SvrData.cpp
  3. 98
      lwrserv/SvrHandling.cpp
  4. 22
      lwrserv/Trajectroy.cpp
  5. 355
      lwrserv/friremote.cpp
  6. 14
      lwrserv/include/SvrData.h
  7. 2
      lwrserv/include/Trajectroy.h
  8. 5
      lwrserv/program.cpp

20
lwrserv/LinearTrajectory.cpp
View File

@ -4,17 +4,18 @@
template <unsigned SIZE> template <unsigned SIZE>
LinearJointTrajectory<SIZE>::LinearJointTrajectory( LinearJointTrajectory<SIZE>::LinearJointTrajectory(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement_
float sampleTimeMs,
Vec<float,SIZE> maxJointVelocity,
Vec<float,SIZE> maxJointAcceleration,
Vec<float,SIZE> jointStart,
Vec<float,SIZE> jointEnd
) )
{ {
Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
LinearJointTrajectory(
steps_,
totalTime_,
jointMovement_,
velocity);
//
maxJointLocalVelocity = maxJointVelocity * sampleTimeMs;
maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs;
// calculate sample count
} }
template <unsigned SIZE> template <unsigned SIZE>
@ -25,6 +26,7 @@ LinearJointTrajectory<SIZE>::LinearJointTrajectory(
Vec<float,SIZE> velocity Vec<float,SIZE> velocity
) )
{ {
Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
super(steps_, totalTime_, jointMovement); super(steps_, totalTime_, jointMovement);
if ( velocity > jointMovement/totalTime_) if ( velocity > jointMovement/totalTime_)

82
lwrserv/SvrData.cpp
View File

@ -20,8 +20,8 @@ SvrData::SvrData(void)
double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.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}; double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0};
robot.currentVelocity = 20;
robot.currentAcceleration = 10;
robot.currentVelocityPercentage = 20;
robot.currentAccelerationPercentage = 10;
for (unsigned int i = 0; i < JOINT_NUMBER; ++i) for (unsigned int i = 0; i < JOINT_NUMBER; ++i)
{ {
@ -191,40 +191,38 @@ VecJoint SvrData::getMaxTorque()
VecJoint SvrData::getMaxAcceleration() VecJoint SvrData::getMaxAcceleration()
{ {
VecJoint buff;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
buff = robot.max.accelaration;
retval = robot.max.accelaration;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return buff;
return retval;
} }
VecJoint SvrData::getMaxVelocity() VecJoint SvrData::getMaxVelocity()
{ {
VecJoint buff;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
buff = robot.max.velocity;
retval = robot.max.velocity;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return buff;
return retval;
} }
VecJoint SvrData::getMaxRange() VecJoint SvrData::getMaxRange()
{ {
VecJoint buff;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
buff = robot.max.range;
retval = robot.max.range;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return buff;
return retval;
} }
bool SvrData::checkJointRange(VecJoint vectorToCheck) bool SvrData::checkJointRange(VecJoint vectorToCheck)
{ {
bool retval = false;
// save temporal the max range for short time lock
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
Vec<float,LBR_MNJ> maxJointRange; Vec<float,LBR_MNJ> maxJointRange;
maxJointRange = robot.max.range; maxJointRange = robot.max.range;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
// check if the value is in plus or minus range
for (int i = 0; i<JOINT_NUMBER; i++) for (int i = 0; i<JOINT_NUMBER; i++)
{ {
if (abs(vectorToCheck(i)) > abs(maxJointRange(i))) if (abs(vectorToCheck(i)) > abs(maxJointRange(i)))
@ -234,42 +232,76 @@ bool SvrData::checkJointRange(VecJoint vectorToCheck)
} }
} }
// no join angle is too big // no join angle is too big
return false;
return true;
} }
int SvrData::setRobotVelocity(float newVelocity)
int SvrData::setRobotVelocityPercentage(float newVelocityPercentage)
{ {
unsigned int retval = 0; unsigned int retval = 0;
// percentage is to low
if (newVelocityPercentage <= 0.0f)
return -1;
// percentage is to high
if (newVelocityPercentage > 100.0f)
return -2;
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
robot.currentVelocity = newVelocity;
robot.currentVelocityPercentage = newVelocityPercentage;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
float SvrData::getRobotVelocity()
VecJoint SvrData::getRobotVelocity()
{
float percent = 0.0f;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock);
percent = robot.currentVelocityPercentage / 100.0f;
retval = robot.max.velocity * percent;
pthread_mutex_unlock(&dataLock);
return retval;
}
float SvrData::getRobotVelocityPercentage()
{ {
float retval = 0; float retval = 0;
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
retval = robot.currentVelocity;
retval = robot.currentVelocityPercentage;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
int SvrData::setRobotAcceleration(float newAcceleration)
int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage)
{ {
unsigned int retval = 0;
int retval = 0;
// percentage is to low
if (newAccelerationPercentage <= 0.0f)
return -1;
// percentage is to high
if (newAccelerationPercentage > 100.0f)
return -2;
pthread_mutex_lock(&dataLock);
robot.currentAccelerationPercentage = newAccelerationPercentage;
pthread_mutex_unlock(&dataLock);
return retval;
}
VecJoint SvrData::getRobotAcceleration()
{
float percent = 0;
VecJoint retval(0.0f);
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
robot.currentAcceleration = newAcceleration;
percent = robot.currentAccelerationPercentage / 100.0f;
retval = robot.max.accelaration * percent;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }
float SvrData::getRobotAcceleration()
float SvrData::getRobotAccelerationPercentage()
{ {
float retval = 0; float retval = 0;
pthread_mutex_lock(&dataLock); pthread_mutex_lock(&dataLock);
retval = robot.currentAcceleration;
retval = robot.currentAccelerationPercentage;
pthread_mutex_unlock(&dataLock); pthread_mutex_unlock(&dataLock);
return retval; return retval;
} }

98
lwrserv/SvrHandling.cpp
View File

@ -10,6 +10,7 @@
#include "SvrHandling.h" #include "SvrHandling.h"
#include "SvrData.h" #include "SvrData.h"
#include "Trajectroy.h" #include "Trajectroy.h"
#include <boost/thread/thread.hpp>
void printUsage(SocketObject& client) void printUsage(SocketObject& client)
{ {
@ -46,7 +47,6 @@ void debugMat(Mat4f M)
double* kukaBackwardCalc(double M_[12], float arg[3]) double* kukaBackwardCalc(double M_[12], float arg[3])
{ {
double* Joints = new double[7]; double* Joints = new double[7];
//DH-Parameter //DH-Parameter
float alp[7] = {-M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, 0.0f}; float alp[7] = {-M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, -M_PI/2.0f, M_PI/2.0f, 0.0f};
@ -460,6 +460,7 @@ void SvrHandling::handle(SocketObject& client)
} }
} }
string SvrHandling::timestamp() string SvrHandling::timestamp()
{ {
time_t rawtime; time_t rawtime;
@ -480,15 +481,18 @@ void SvrHandling::GetPositionJoints(SocketObject& client)
string out = ""; string out = "";
stringstream ss (stringstream::in | stringstream::out); stringstream ss (stringstream::in | stringstream::out);
// get current joint positions from database
VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos();
// disassemble command
// assemble command feadback
for (int i=0; i< JOINT_NUMBER; i++) for (int i=0; i< JOINT_NUMBER; i++)
{ {
ss.str(""); ss.str("");
ss << (jointPos(i)*180/M_PI); ss << (jointPos(i)*180/M_PI);
out += ss.str() + " "; out += ss.str() + " ";
} }
// send msg to client
client.Send(out); client.Send(out);
} }
@ -497,17 +501,21 @@ void SvrHandling::GetPositionHomRowWise(SocketObject& client)
string out = ""; string out = "";
stringstream ss (stringstream::in | stringstream::out); stringstream ss (stringstream::in | stringstream::out);
// get current Cartesianpositions from database
MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos(); MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos();
// assemble command feadback row wise
for (int i=0; i< FRI_CART_FRM_DIM; i++) for (int i=0; i< FRI_CART_FRM_DIM; i++)
{ {
int row = i / 4;
int column = i % 4;
int row = i % 4;
int column = i / 4;
ss.str(""); ss.str("");
ss << (cartPos(row,column)); ss << (cartPos(row,column));
out += ss.str() + " "; out += ss.str() + " ";
} }
// send msg to client
client.Send(out); client.Send(out);
} }
@ -570,33 +578,20 @@ void SvrHandling::MovePTPJoints(SocketObject& client, string& args)
VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos(); VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos();
class Trajectory<JOINT_NUMBER>* newTrajectory; 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());
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
VecJoint maxJointVelocity = SvrData::getInstance()->getMaxVelocity();
VecJoint maxJointAcceleration = SvrData::getInstance()->getMaxAcceleration();
newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(steps, totalTime, newJointPos);
newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos);
__MSRCMDJNTPOS = true; __MSRCMDJNTPOS = true;
//Wait to end of movement //Wait to end of movement
while (1)
while (true)
{ {
if (!__MSRCMDJNTPOS)
{
if (SvrData::getInstance()->getMovementDone() == true)
break; break;
}
boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) );
} }
client.Send(SVR_TRUE_RSP); client.Send(SVR_TRUE_RSP);
} }
@ -789,7 +784,8 @@ void SvrHandling::StopPotFieldMode(SocketObject& client, string& args)
client.Send(SVR_TRUE_RSP); client.Send(SVR_TRUE_RSP);
} }
void SvrHandling::SetPos(SocketObject& client, string& args){
void SvrHandling::SetPos(SocketObject& client, string& args)
{
string buf; string buf;
double temp[15]; double temp[15];
@ -868,57 +864,55 @@ void SvrHandling::SetPos(SocketObject& client, string& args){
void SvrHandling::SetJoints(SocketObject& client, string& args) void SvrHandling::SetJoints(SocketObject& client, string& args)
{ {
string buf; string buf;
double temp[15];
VecJoint newJointPos(0.0f);
stringstream ss(args); stringstream ss(args);
stringstream ss2f; stringstream ss2f;
vector<string> tokens;
int i = 0; int i = 0;
// convert to Joint vector
while (ss >> buf) while (ss >> buf)
{ {
if (i>LBR_MNJ-1)
// to many input arguments or Joints
if (i>=JOINT_NUMBER)
{ {
client.Send(SVR_FALSE_RSP);
client.Send(SVR_INVALID_ARGUMENT_COUNT);
return; return;
} }
tokens.push_back(buf);
// convert string to float
ss2f.flush(); ss2f.flush();
ss2f.clear(); ss2f.clear();
ss2f << buf; ss2f << buf;
ss2f >> temp[i];
ss2f >> newJointPos(i);
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; return;
} }
//Check for Joint Range //Check for Joint Range
if (!checkJntRange(temp))
if (!checkJntRange(newJointPos))
{ {
string out = "Error: Joints out of Range!";
client.Send(out);
client.Send(SVR_OUT_OF_RANGE);
return; return;
} }
// Set global Position
__MSRSETPOS = true;
globi = 1;
for (int i=0 ;i<LBR_MNJ; i++)
{
MSRCMDJNTPOS[i]=temp[i];
}
__MSRSETPOS = false;
sleep(5);
globi = 0;
//__MSRCMDJNTPOS = true;
// calculate trajectory from last commanded joint position
VecJoint currentComandedJoint = SvrData::getInstance()->getCommandedJointPos();
////Wait to end of movement
//while (1){
// if (!__MSRCMDJNTPOS){
// break;}
//}
// set new trajectory
// Set new target Position and also calculate cart pos
SvrData::getInstance()->setCommandedJointPos(newJointPos);
// TODO calculate and calculate position in Cartesian coordinates
// wait till position is reached
// return positiv response to client
client.Send(SVR_TRUE_RSP); client.Send(SVR_TRUE_RSP);
} }

22
lwrserv/Trajectroy.cpp
View File

@ -4,22 +4,30 @@
template <unsigned SIZE> template <unsigned SIZE>
Trajectory<SIZE>::Trajectory( Trajectory<SIZE>::Trajectory(
unsigned int steps_,
float totalTime_,
Vec<float,SIZE> jointMovement
float sampleTimeMs,
Vec<float,SIZE> maxJointVelocity,
Vec<float,SIZE> maxJointAcceleration,
Vec<float,SIZE> jointStart,
Vec<float,SIZE> jointEnd
) )
{ {
movementType = MovementJointBased; movementType = MovementJointBased;
steps = steps_;
steps = 1;
currentStep = 0; currentStep = 0;
totalTime = totalTime_;
totalTime = steps * sampleTimeMs;
nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);
nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps);
nodes[0].jointPos = jointEnd;
nodes[0].cartPos = kukaBackwardCalc();
nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs;
nodes[0].acceleration = Vec<float,SIZE>(9999.0f);
// unused // unused
(void) jointMovement;
(void)jointStart;
(void)jointEnd;
} }
template <unsigned SIZE> template <unsigned SIZE>

355
lwrserv/friremote.cpp
View File

@ -29,8 +29,8 @@
[FH]}} [FH]}}
*/ */
/** /**
\addtogroup friRemoteLib
\brief Library for FRI (FastResearchInterface)
\addtogroup friRemoteLib
\brief Library for FRI (FastResearchInterface)
*/ */
/* @{ */ /* @{ */
@ -47,32 +47,32 @@
friRemote::friRemote(int port,char *hintToRemoteHost):remote(port,hintToRemoteHost) friRemote::friRemote(int port,char *hintToRemoteHost):remote(port,hintToRemoteHost)
{ {
//std::cout << __FUNCTION__ << " " <<port <<std::endl;
//std::cout << "FRI Version " << FRI_MAJOR_VERSION << "." << FRI_SUB_VERSION << "." <<FRI_DATAGRAM_ID_CMD << "." <<FRI_DATAGRAM_ID_MSR << std::endl;
memset((void*)(& msr),0x0,sizeof(msr));
memset((void*)(& cmd), 0x0,sizeof(cmd));
{
// do checks, whether the interface - and the host meets the requirements
FRI_PREPARE_CHECK_BYTE_ORDER;
if (!FRI_CHECK_BYTE_ORDER_OK)
{
std::cerr << __FUNCTION__<<"Byte order on your system is not appropriate - expect deep trouble" << std::endl;
}
if (!FRI_CHECK_SIZES_OK)
{
std::cout << __FUNCTION__<<"Sizes of datastructures not appropriate - expect even deeper trouble" << std::endl;
}
}
//std::cout << __FUNCTION__ << " " <<port <<std::endl;
//std::cout << "FRI Version " << FRI_MAJOR_VERSION << "." << FRI_SUB_VERSION << "." <<FRI_DATAGRAM_ID_CMD << "." <<FRI_DATAGRAM_ID_MSR << std::endl;
memset((void*)(& msr),0x0,sizeof(msr));
memset((void*)(& cmd), 0x0,sizeof(cmd));
{
// do checks, whether the interface - and the host meets the requirements
FRI_PREPARE_CHECK_BYTE_ORDER;
if (!FRI_CHECK_BYTE_ORDER_OK)
{
std::cerr << __FUNCTION__<<"Byte order on your system is not appropriate - expect deep trouble" << std::endl;
}
if (!FRI_CHECK_SIZES_OK)
{
std::cout << __FUNCTION__<<"Sizes of datastructures not appropriate - expect even deeper trouble" << std::endl;
}
}
} }
friRemote::~friRemote() friRemote::~friRemote()
{ {
std::cout << __FUNCTION__ << " bye for now " << std::endl;
std::cout << __FUNCTION__ << " bye for now " << std::endl;
} }
int friRemote::doReceiveData() int friRemote::doReceiveData()
{ {
return remote.Recv(&msr);
return remote.Recv(&msr);
} }
@ -94,41 +94,41 @@ int friRemote::doSendData()
*/ */
int friRemote::doDataExchange() int friRemote::doDataExchange()
{ {
doSendData();
return doReceiveData();
doSendData();
return doReceiveData();
} }
/** automatically do data exchange, if not otherwise specified */ /** automatically do data exchange, if not otherwise specified */
int friRemote::doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange) int friRemote::doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange)
{ {
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
{
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
}
}
else
{
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=newJntPosition[i];
}
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
{
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
}
}
else
{
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=newJntPosition[i];
}
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
} }
@ -136,147 +136,144 @@ int friRemote::doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExc
if flagDataExchange is set to false, call doDataExchange() if flagDataExchange is set to false, call doDataExchange()
or doReceiveData()/doSendData() on your own or doReceiveData()/doSendData() on your own
IN: newJntPosition - joint positions IN: newJntPosition - joint positions
newJntStiff - joint stiffness (Spring factor)
newJntDamp - joint damping (Damping factor)
newJntAddTorque - additional torque
newJntStiff - joint stiffness (Spring factor)
newJntDamp - joint damping (Damping factor)
newJntAddTorque - additional torque
Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the
value is ignored -> the respective cmd.cmd.cmdFlags field is set properly
value is ignored -> the respective cmd.cmd.cmdFlags field is set properly
Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !! Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
*/ */
int friRemote::doJntImpedanceControl(const float newJntPosition[LBR_MNJ], int friRemote::doJntImpedanceControl(const float newJntPosition[LBR_MNJ],
const float newJntStiff[LBR_MNJ],
const float newJntDamp[LBR_MNJ],
const float newJntAddTorque[LBR_MNJ],
bool flagDataExchange)
const float newJntStiff[LBR_MNJ],
const float newJntDamp[LBR_MNJ],
const float newJntAddTorque[LBR_MNJ],
bool flagDataExchange)
{ {
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=0;
if (newJntPosition)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
{
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
}
}
else
{
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntPos[i]=newJntPosition[i];
}
}
if (newJntStiff)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntStiffness[i]=newJntStiff[i];
}
if (newJntDamp)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntDamping[i]=newJntDamp[i];
}
if (newJntAddTorque)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.addJntTrq[i]=newJntAddTorque[i];
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=0;
if (newJntPosition)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
{
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
}
}
else
{
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntPos[i]=newJntPosition[i];
}
}
if (newJntStiff)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntStiffness[i]=newJntStiff[i];
}
if (newJntDamp)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntDamping[i]=newJntDamp[i];
}
if (newJntAddTorque)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.addJntTrq[i]=newJntAddTorque[i];
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
} }
/** automatically do data exchange, if not otherwise specified
if flagDataExchange is set to false, call doDataExchange()
or doReceiveData()/doSendData() on your own
IN: newJntPosition - joint positions
newJntStiff - joint stiffness (Spring factor)
newJntDamp - joint damping (Damping factor)
newJntAddTorque - additional torque
Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the
value is ignored -> the respective cmd.cmd.cmdFlags field is set properly
Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
*/
/** automatically do data exchange, if not otherwise specified
if flagDataExchange is set to false, call doDataExchange()
or doReceiveData()/doSendData() on your own
IN: newJntPosition - joint positions
newJntStiff - joint stiffness (Spring factor)
newJntDamp - joint damping (Damping factor)
newJntAddTorque - additional torque
Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the
value is ignored -> the respective cmd.cmd.cmdFlags field is set properly
Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
*/
int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], int friRemote::doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM],
const float newCartStiff[FRI_CART_VEC],
const float newCartDamp[FRI_CART_VEC],
const float newAddTcpFT[FRI_CART_VEC],
const float newJntNullspace[LBR_MNJ],
bool flagDataExchange)
const float newCartStiff[FRI_CART_VEC],
const float newCartDamp[FRI_CART_VEC],
const float newAddTcpFT[FRI_CART_VEC],
const float newJntNullspace[LBR_MNJ],
bool flagDataExchange)
{ {
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=0;
if ( newCartPosition )
{
cmd.cmd.cmdFlags|=FRI_CMD_CARTPOS;
for ( int i = 0; i < FRI_CART_FRM_DIM; i++)
{
cmd.cmd.cartPos[i]=newCartPosition[i];
}
}
if ( newCartStiff)
{
cmd.cmd.cmdFlags|=FRI_CMD_CARTSTIFF;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.cartStiffness[i]=newCartStiff[i];
}
}
if ( newCartDamp)
{
cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP;
;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.cartDamping[i]=newCartDamp[i];
}
}
if ( newAddTcpFT)
{
cmd.cmd.cmdFlags|=FRI_CMD_TCPFT;
;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.addTcpFT[i]=newAddTcpFT[i];
}
}
if (newJntNullspace)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntPos[i]=newJntNullspace[i];
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=0;
if ( newCartPosition )
{
cmd.cmd.cmdFlags|=FRI_CMD_CARTPOS;
for ( int i = 0; i < FRI_CART_FRM_DIM; i++)
{
cmd.cmd.cartPos[i]=newCartPosition[i];
}
}
if ( newCartStiff)
{
cmd.cmd.cmdFlags|=FRI_CMD_CARTSTIFF;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.cartStiffness[i]=newCartStiff[i];
}
}
if ( newCartDamp)
{
cmd.cmd.cmdFlags|=FRI_CMD_CARTDAMP;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.cartDamping[i]=newCartDamp[i];
}
}
if ( newAddTcpFT)
{
cmd.cmd.cmdFlags|=FRI_CMD_TCPFT;
for ( int i = 0; i < FRI_CART_VEC; i++)
{
cmd.cmd.addTcpFT[i]=newAddTcpFT[i];
}
}
if (newJntNullspace)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntPos[i]=newJntNullspace[i];
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
} }

14
lwrserv/include/SvrData.h
View File

@ -38,8 +38,8 @@ private:
VecJoint torque; VecJoint torque;
VecJoint range; VecJoint range;
} max; } max;
float currentVelocity;
float currentAcceleration;
float currentVelocityPercentage;
float currentAccelerationPercentage;
} robot; } robot;
struct { struct {
@ -97,11 +97,13 @@ public:
bool checkJointRange(VecJoint vectorToCheck); bool checkJointRange(VecJoint vectorToCheck);
float getRobotVelocity();
int setRobotVelocity(float newVelocity);
VecJoint getRobotVelocity();
float getRobotVelocityPercentage();
int setRobotVelocityPercentage(float newVelocityPercentage);
float getRobotAcceleration();
int setRobotAcceleration(float newVelocity);
VecJoint getRobotAcceleration();
float getRobotAccelerationPercentage();
int setRobotAccelerationPercentage(float newAccelerationPercentage);
unsigned int getJoints(); unsigned int getJoints();

2
lwrserv/include/Trajectroy.h
View File

@ -57,7 +57,7 @@ class Trajectory
struct trajectoryNode struct trajectoryNode
{ {
Vec<float,SIZE> jointPos; Vec<float,SIZE> jointPos;
Mat<float,4>* cartPos;
Mat<float,4> cartPos;
Vec<float,SIZE> velocity; Vec<float,SIZE> velocity;
Vec<float,SIZE> acceleration; Vec<float,SIZE> acceleration;
}; };

5
lwrserv/program.cpp
View File

@ -9,11 +9,6 @@
#include "SvrData.h" #include "SvrData.h"
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
void printMat(Mat4f M)
{
cout << M << endl;
}
float* abctomat(float a, float b, float c) float* abctomat(float a, float b, float c)
{ {
Mat4f rx; Mat4f rx;

Write Preview
Loading…
Cancel
Save