philipp schoenberger
10 years ago
14 changed files with 1097 additions and 1054 deletions
-
2lwrserv/Makefile
-
40lwrserv/SocketObject.cpp
-
60lwrserv/SvrData.cpp
-
881lwrserv/SvrHandling.cpp
-
38lwrserv/SvrHandling.h
-
601lwrserv/commands.cpp
-
36lwrserv/global.cpp
-
24lwrserv/header.h
-
55lwrserv/include/SvrData.h
-
2lwrserv/include/Trajectroy.h
-
49lwrserv/include/commands.h
-
244lwrserv/include/lwr4.h
-
96lwrserv/include/robot.h
-
23lwrserv/program.cpp
@ -0,0 +1,601 @@ |
|||||
|
#include "commands.h"
|
||||
|
#include "robot.h"
|
||||
|
#include "lwr4.h"
|
||||
|
#include "SocketObject.h"
|
||||
|
#include "Trajectroy.h"
|
||||
|
#include "SvrHandling.h"
|
||||
|
#include "SvrData.h"
|
||||
|
|
||||
|
#include <stdlib.h>
|
||||
|
#include <string>
|
||||
|
#include "mat.h"
|
||||
|
#include "vec.h"
|
||||
|
|
||||
|
#include "StringTool.h"
|
||||
|
#include <iostream>
|
||||
|
#include <iterator>
|
||||
|
#include <algorithm>
|
||||
|
|
||||
|
#include <boost/thread/thread.hpp>
|
||||
|
|
||||
|
void getPositionJoints(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
// unused
|
||||
|
(void) arg; |
||||
|
|
||||
|
std::string out = ""; |
||||
|
std::stringstream ss (std::stringstream::in | std::stringstream::out); |
||||
|
|
||||
|
// get current joint positions from database
|
||||
|
Robot::VecJoint jointPos = SvrData::getInstance()->getMessuredJointPos(); |
||||
|
|
||||
|
// assemble command feadback
|
||||
|
for (int i=0; i< JOINT_NUMBER; i++) |
||||
|
{ |
||||
|
ss.str(""); |
||||
|
ss << (jointPos(i)*180/M_PI); |
||||
|
out += ss.str() + " "; |
||||
|
} |
||||
|
|
||||
|
// send msg to client
|
||||
|
client.Send(out); |
||||
|
} |
||||
|
|
||||
|
void getPositionHomRowWise(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
// unused
|
||||
|
(void) arg; |
||||
|
|
||||
|
std::string out = ""; |
||||
|
std::stringstream ss (std::stringstream::in | std::stringstream::out); |
||||
|
|
||||
|
// get current Cartesian positions from database
|
||||
|
MatCarthesian cartPos = SvrData::getInstance()->getMessuredCartPos(); |
||||
|
|
||||
|
// assemble command feedback row wise
|
||||
|
for (int i=0; i< FRI_CART_FRM_DIM; i++) |
||||
|
{ |
||||
|
int row = i % 4; |
||||
|
int column = i / 4; |
||||
|
|
||||
|
ss.str(""); |
||||
|
ss << (cartPos(row,column)); |
||||
|
out += ss.str() + " "; |
||||
|
} |
||||
|
|
||||
|
// send msg to client
|
||||
|
client.Send(out); |
||||
|
} |
||||
|
|
||||
|
void getForceTorqueTcp(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
// unused
|
||||
|
(void) arg; |
||||
|
|
||||
|
std::string out = ""; |
||||
|
std::stringstream ss (std::stringstream::in | std::stringstream::out); |
||||
|
|
||||
|
VecTorque torque = SvrData::getInstance()->getMessuredForceTorque(); |
||||
|
for (int i=0; i< FRI_CART_VEC; i++) |
||||
|
{ |
||||
|
ss.str(""); |
||||
|
ss << torque(i); |
||||
|
out += ss.str() + " "; |
||||
|
} |
||||
|
client.Send(out); |
||||
|
} |
||||
|
|
||||
|
void movePTPJoints(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
std::string buf; |
||||
|
|
||||
|
std::stringstream ss(arg); |
||||
|
std::stringstream ss2f; |
||||
|
|
||||
|
// convert string to joint vector
|
||||
|
Robot::VecJoint newJointPos(0.0f); |
||||
|
int i = 0; |
||||
|
while (ss >> buf) |
||||
|
{ |
||||
|
// to many joint values
|
||||
|
if (i>=JOINT_NUMBER) |
||||
|
{ |
||||
|
client.Send(SVR_INVALID_ARGUMENT_COUNT); |
||||
|
return; |
||||
|
} |
||||
|
ss2f.flush(); |
||||
|
ss2f.clear(); |
||||
|
ss2f << buf; |
||||
|
ss2f >> newJointPos(i); |
||||
|
i++; |
||||
|
} |
||||
|
|
||||
|
// to less joint values
|
||||
|
if (i!=JOINT_NUMBER) |
||||
|
{ |
||||
|
client.Send(SVR_INVALID_ARGUMENT_COUNT); |
||||
|
return ; |
||||
|
} |
||||
|
|
||||
|
|
||||
|
//check for joint range
|
||||
|
if (!SvrData::getInstance()->checkJointRange(newJointPos)) |
||||
|
{ |
||||
|
std::string out = "Error: Joints out of Range!"; |
||||
|
client.Send(SVR_OUT_OF_RANGE); |
||||
|
return; |
||||
|
} |
||||
|
//set new target for next trajectory
|
||||
|
Robot::VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos(); |
||||
|
class Trajectory<JOINT_NUMBER>* newTrajectory; |
||||
|
|
||||
|
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); |
||||
|
Robot::VecJoint maxJointVelocity = SvrData::getInstance()->getRobotVelocity(); |
||||
|
Robot::VecJoint maxJointAcceleration = SvrData::getInstance()->getRobotAcceleration(); |
||||
|
|
||||
|
//newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
|
||||
|
newTrajectory = (class Trajectory<JOINT_NUMBER>*) new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); |
||||
|
|
||||
|
// add trajectory to queue for sender thread
|
||||
|
SvrData::getInstance()->pushTrajectory(newTrajectory); |
||||
|
|
||||
|
SvrData::getInstance()->setCommandedJointPos(newJointPos); |
||||
|
|
||||
|
//Wait to end of movement
|
||||
|
while (true) |
||||
|
{ |
||||
|
if (SvrData::getInstance()->getTrajectoryDone() == true) |
||||
|
break; |
||||
|
boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) ); |
||||
|
} |
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void moveHomRowWiseStatus(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
std::string buf; |
||||
|
double temp[15]; |
||||
|
std::stringstream ss(arg); |
||||
|
std::stringstream ss2f; |
||||
|
int argc = 0; |
||||
|
|
||||
|
// convert position parameters from string to float
|
||||
|
while (ss >> buf) |
||||
|
{ |
||||
|
// only need 15 parameters not more
|
||||
|
if (argc>15-1) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
ss2f.flush(); |
||||
|
ss2f.clear(); |
||||
|
ss2f << buf; |
||||
|
ss2f >> temp[argc]; |
||||
|
argc++; |
||||
|
} |
||||
|
|
||||
|
// check for exactly 15 parameters
|
||||
|
if (argc != 15) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return ; |
||||
|
} |
||||
|
|
||||
|
// first part is homegenous position koordinates
|
||||
|
MatCarthesian newCartPos(0.0f, 1.0f); |
||||
|
for (int i=0 ;i<3; i++) |
||||
|
{ |
||||
|
for (int j=0;j<4;j++) |
||||
|
{ |
||||
|
newCartPos(i,j)=temp[i*4+j]; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// extract elbow flip and hand parameter
|
||||
|
struct Robot::RobotConfig configurationParameter; |
||||
|
configurationParameter.elbow = temp[12] == 1.0f; |
||||
|
configurationParameter.flip = temp[13] == 1.0f; |
||||
|
configurationParameter.j1os = temp[14] == 1.0f; |
||||
|
|
||||
|
//Backward Calculation
|
||||
|
Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); |
||||
|
|
||||
|
|
||||
|
//Check for Joint Range
|
||||
|
if (!SvrData::getInstance()->checkJointRange(newJointPos)) |
||||
|
{ |
||||
|
client.Send(SVR_OUT_OF_RANGE); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// calculate trajectory
|
||||
|
Robot::VecJoint prevJointPos = SvrData::getInstance()->getCommandedJointPos(); |
||||
|
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs(); |
||||
|
Robot::VecJoint maxJointVelocity = SvrData::getInstance()->getRobotVelocity(); |
||||
|
Robot::VecJoint maxJointAcceleration = SvrData::getInstance()->getRobotAcceleration(); |
||||
|
std::cout << "max velo is " << maxJointVelocity ; |
||||
|
|
||||
|
class Trajectory<JOINT_NUMBER>* newTrajectory = new LinearJointTrajectory<JOINT_NUMBER>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); |
||||
|
|
||||
|
// set new commanded position for next trajectory
|
||||
|
SvrData::getInstance()->setCommandedJointPos(newJointPos); |
||||
|
|
||||
|
// add new trajectory to position
|
||||
|
SvrData::getInstance()->pushTrajectory(newTrajectory); |
||||
|
|
||||
|
//Wait to end of movement
|
||||
|
while (true) |
||||
|
{ |
||||
|
if (SvrData::getInstance()->getTrajectoryDone() == true) |
||||
|
break; |
||||
|
boost::this_thread::sleep( boost::posix_time::milliseconds(sampleTimeMs) ); |
||||
|
} |
||||
|
|
||||
|
// send a positive client feedback
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void setSpeed(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
std::string buf; |
||||
|
float newVelocityPercentage; |
||||
|
int argc = 0; |
||||
|
std::stringstream ss(arg); |
||||
|
std::stringstream ss2f; |
||||
|
|
||||
|
// no speed argument
|
||||
|
if (arg == "") |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// convert speed argument to float
|
||||
|
while (ss >> buf) |
||||
|
{ |
||||
|
// only need one speed argument
|
||||
|
if (argc>0) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
ss2f.flush(); |
||||
|
ss2f.clear(); |
||||
|
ss2f << buf; |
||||
|
ss2f >> newVelocityPercentage; |
||||
|
argc++; |
||||
|
} |
||||
|
|
||||
|
// check for a valid range for the new Velocity
|
||||
|
if ((newVelocityPercentage <= 0.0f)||(newVelocityPercentage >100.0f)) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// save new velocity into the database
|
||||
|
SvrData::getInstance()->setRobotVelocityPercentage(newVelocityPercentage); |
||||
|
|
||||
|
// signal a positive feedback to the client side
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void setAccel(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
std::string buf; |
||||
|
float newAccelerationPercentage; |
||||
|
std::stringstream ss(arg); |
||||
|
std::stringstream ss2f; |
||||
|
int argc = 0; |
||||
|
|
||||
|
// need at least one argument
|
||||
|
if (arg == "") |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// convert acceleration string into float
|
||||
|
while (ss >> buf) |
||||
|
{ |
||||
|
// only one argument is necessary
|
||||
|
if (argc>0) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
ss2f.flush(); |
||||
|
ss2f.clear(); |
||||
|
ss2f << buf; |
||||
|
ss2f >> newAccelerationPercentage; |
||||
|
argc++; |
||||
|
} |
||||
|
|
||||
|
// check for a valid rang for the robot acceleration
|
||||
|
if ((newAccelerationPercentage < 1.0f)||(newAccelerationPercentage >100.0f)) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// save new acceleration
|
||||
|
SvrData::getInstance()->setRobotAccelerationPercentage(newAccelerationPercentage); |
||||
|
|
||||
|
// signal a positive client feedback
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void startPotFieldMode(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
// unused
|
||||
|
(void) arg; |
||||
|
#if 0
|
||||
|
__MSRSTARTPOTFIELD = false; |
||||
|
__MSRSTOPPOTFIELD = false; |
||||
|
//Set current Joint Vals
|
||||
|
for (int i=0; i<LBR_MNJ; i++) |
||||
|
{ |
||||
|
MSRCMDJNTPOS[i] = MSRMSRJNTPOS[i]*180/M_PI; |
||||
|
} |
||||
|
__POTFIELDMODE = true; |
||||
|
while (1) |
||||
|
{ |
||||
|
if (!__MSRSTARTPOTFIELD) |
||||
|
{ |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
#endif
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void stopPotFieldMode(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
// unused
|
||||
|
(void) arg; |
||||
|
#if 0
|
||||
|
__POTFIELDMODE = false; |
||||
|
|
||||
|
while (1) |
||||
|
{ |
||||
|
if (!__MSRSTOPPOTFIELD) |
||||
|
{ |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
#endif
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void setPos(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
std::string buf; |
||||
|
double temp[15]; |
||||
|
std::stringstream ss(arg); |
||||
|
std::stringstream ss2f; |
||||
|
int argc = 0; |
||||
|
|
||||
|
// convert position arguments from string to float
|
||||
|
while (ss >> buf) |
||||
|
{ |
||||
|
// only 15 parameters are necessary
|
||||
|
if (argc>=15) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
ss2f.flush(); |
||||
|
ss2f.clear(); |
||||
|
ss2f << buf; |
||||
|
ss2f >> temp[argc]; |
||||
|
argc++; |
||||
|
} |
||||
|
|
||||
|
// check for exactly 15 parameters
|
||||
|
if (argc!=15) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return ; |
||||
|
} |
||||
|
|
||||
|
|
||||
|
// the position is saved in the first parameter part
|
||||
|
MatCarthesian newCartPos(0.0f,1.0f); |
||||
|
for (int i=0 ;i<3; i++) |
||||
|
{ |
||||
|
for (int j=0;j<4;j++) |
||||
|
{ |
||||
|
newCartPos(i,j)=temp[i*4+j]; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// extract elbow flip and hand parameter
|
||||
|
struct Robot::RobotConfig configurationParameter; |
||||
|
configurationParameter.elbow = temp[12] == 1.0f; |
||||
|
configurationParameter.flip = temp[13] == 1.0f; |
||||
|
configurationParameter.j1os = temp[14] == 1.0f; |
||||
|
|
||||
|
//Backward Calculation
|
||||
|
Robot::VecJoint newJointPos = LWR4::backwardCalc(newCartPos, configurationParameter); |
||||
|
|
||||
|
//Check for a valid joint range
|
||||
|
if (!SvrData::getInstance()->checkJointRange(newJointPos)) |
||||
|
{ |
||||
|
client.Send(SVR_OUT_OF_RANGE); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// calculate new trajectory for next position
|
||||
|
|
||||
|
// set new commanded Position for next Trajectory
|
||||
|
SvrData::getInstance()->setCommandedJointPos(newJointPos); |
||||
|
|
||||
|
//Wait to end of movement
|
||||
|
|
||||
|
// send positive client feedback
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void setJoints(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
std::string buf; |
||||
|
Robot::VecJoint newJointPos(0.0f); |
||||
|
std::stringstream ss(arg); |
||||
|
std::stringstream ss2f; |
||||
|
int argc = 0; |
||||
|
|
||||
|
// convert to Joint vector
|
||||
|
while (ss >> buf) |
||||
|
{ |
||||
|
// to many input arguments or Joints
|
||||
|
if (argc>=JOINT_NUMBER) |
||||
|
{ |
||||
|
client.Send(SVR_INVALID_ARGUMENT_COUNT); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// convert string to float
|
||||
|
ss2f.flush(); |
||||
|
ss2f.clear(); |
||||
|
ss2f << buf; |
||||
|
ss2f >> newJointPos(argc); |
||||
|
argc++; |
||||
|
} |
||||
|
|
||||
|
// to less joint values
|
||||
|
if (argc != JOINT_NUMBER) |
||||
|
{ |
||||
|
client.Send(SVR_INVALID_ARGUMENT_COUNT); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
//Check for Joint Range
|
||||
|
if (!SvrData::getInstance()->checkJointRange(newJointPos)) |
||||
|
{ |
||||
|
client.Send(SVR_OUT_OF_RANGE); |
||||
|
return; |
||||
|
} |
||||
|
|
||||
|
// calculate trajectory from last commanded joint position
|
||||
|
Robot::VecJoint currentComandedJoint = SvrData::getInstance()->getCommandedJointPos(); |
||||
|
|
||||
|
// set new trajectory
|
||||
|
|
||||
|
// Set new target Position and also calculate Cartesian position
|
||||
|
SvrData::getInstance()->setCommandedJointPos(newJointPos); |
||||
|
// TODO calculate and calculate position in Cartesian coordinates
|
||||
|
|
||||
|
// wait till position is reached
|
||||
|
|
||||
|
// return positive response to client
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void moveCartesian(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
#if 0
|
||||
|
std::string buf; |
||||
|
float temp[15]; |
||||
|
std::stringstream ss(arg); |
||||
|
std::stringstream ss2f; |
||||
|
int argc = 0; |
||||
|
|
||||
|
// convert Cartesian coordinates from string into float
|
||||
|
while (ss >> buf) |
||||
|
{ |
||||
|
// only need maximum 16 coordinates for a homogeneous matrix
|
||||
|
if (argc>=15) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return; |
||||
|
} |
||||
|
ss2f.flush(); |
||||
|
ss2f.clear(); |
||||
|
ss2f << buf; |
||||
|
ss2f >> temp[argc]; |
||||
|
argc++; |
||||
|
} |
||||
|
|
||||
|
// check for exactly 15 coordinates
|
||||
|
if (argc != 15) |
||||
|
{ |
||||
|
client.Send(SVR_FALSE_RSP); |
||||
|
return ; |
||||
|
} |
||||
|
|
||||
|
float configurationParameter[3]; |
||||
|
for (int i=0;i<3;i++) |
||||
|
{ |
||||
|
configurationParameter[i] = temp[12+i]; |
||||
|
} |
||||
|
|
||||
|
//Calculating end-effector position for target "TRobot"
|
||||
|
Mat4f Tsurface; |
||||
|
Tsurface = vecToMat(temp); |
||||
|
Mat4f TRobot; |
||||
|
TRobot = Tsurface; |
||||
|
|
||||
|
//Calculating end-effector position for approach position "ApRobot"
|
||||
|
Mat4f ApRobot; |
||||
|
Mat4f ApSurface; |
||||
|
Mat4f TransZ; |
||||
|
// 100mm approach position
|
||||
|
TransZ = getTranslation(0,0,-0.1); |
||||
|
ApSurface = Tsurface * TransZ; |
||||
|
ApRobot = ApSurface ; |
||||
|
float* tempVec = NULL; |
||||
|
tempVec = matToVec(TRobot); |
||||
|
//tempVec = matToVec(ApRobot);
|
||||
|
|
||||
|
#endif
|
||||
|
// send a positive client feedback
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
|
||||
|
void quit(SocketObject& client , std::string& arg) |
||||
|
{ |
||||
|
//unused
|
||||
|
(void) arg; |
||||
|
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
client.Disconnect(); |
||||
|
} |
||||
|
|
||||
|
void setTrajectoryType(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
std::string buf; |
||||
|
enum TrajectoryType newType = TrajectoryDefault; |
||||
|
|
||||
|
newType = toEnum(arg); |
||||
|
SvrData::getInstance()->setTrajectoryType(newType); |
||||
|
std::cout << "new trajectory type : "<< toString(newType) << "\n"; |
||||
|
|
||||
|
client.Send(SVR_TRUE_RSP); |
||||
|
} |
||||
|
void getTrajectoryType(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
enum TrajectoryType currType = SvrData::getInstance()->getTrajectoryType(); |
||||
|
std::cout << "curr trajectory type : "<< toString(currType) << "\n"; |
||||
|
|
||||
|
client.Send("current trajectory: " + toString(currType)); |
||||
|
} |
||||
|
|
||||
|
void debug(SocketObject& client, std::string& arg) |
||||
|
{ |
||||
|
|
||||
|
//check = true;
|
||||
|
//float temp[7];
|
||||
|
//while (1){
|
||||
|
// friInst.setToKRLInt(15,1);
|
||||
|
// friInst.doSendData();
|
||||
|
// //client.Send(friInst.getFrmKRLInt(2));
|
||||
|
// for ( int i= 0; i < LBR_MNJ; i++)
|
||||
|
//{
|
||||
|
// temp[i]=friInst.getMsrMsrJntPosition()[i];
|
||||
|
//}
|
||||
|
//friInst.doPositionControl(temp);
|
||||
|
//}
|
||||
|
} |
||||
@ -1,36 +0,0 @@ |
|||||
#if 0
|
|
||||
bool __SVR_CURRENT_JOB = false; |
|
||||
bool __MSRMSRJNTPOS = false; |
|
||||
bool __MSRCMDJNTPOS = false; |
|
||||
bool __MSRCMDPOSE = false; |
|
||||
bool __SETVELOCITY = false; |
|
||||
bool __SETACCEL = false; |
|
||||
bool __DEBUG = false; |
|
||||
bool __POTFIELDMODE = false; |
|
||||
bool __MSRSTARTPOTFIELD = false; |
|
||||
bool __MSRSTOPPOTFIELD = false; |
|
||||
bool __MSRSETPOS = false; |
|
||||
bool __CARTMOVE = false; |
|
||||
bool __DOUS = false; |
|
||||
bool __DOUS2 = false; |
|
||||
|
|
||||
int globi = 0; |
|
||||
|
|
||||
float MSRMSRJNTPOS[7]; |
|
||||
double MSRCMDJNTPOS[7]; |
|
||||
double MSRMSRCARTPOS[12]; |
|
||||
float MSRCMDCARTPOS[12]; |
|
||||
double MSRMSRJACOBIAN[42]; |
|
||||
double MSRMSRFTTCP[6]; |
|
||||
double MSRCMDPOSE[3][4]; |
|
||||
|
|
||||
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
|
|
||||
|
|
||||
|
|
||||
|
|
||||
@ -1,24 +0,0 @@ |
|||||
#include <iostream> |
|
||||
#include <sstream> |
|
||||
#include "defines.h" |
|
||||
#include "SocketObject.h" |
|
||||
#include "SvrHandling.h" |
|
||||
#include "StringTool.h" |
|
||||
#include <string> |
|
||||
#include "pthread.h" |
|
||||
#include "friremote.h" |
|
||||
#include "math.h" |
|
||||
#include <cmath> |
|
||||
#include "mat.h" |
|
||||
#include "vec.h" |
|
||||
//#include "opencv2\opencv.hpp" |
|
||||
//#pragma comment (lib, "opencv_core248d.lib") |
|
||||
|
|
||||
|
|
||||
//debug |
|
||||
#include <algorithm> |
|
||||
//end debug |
|
||||
|
|
||||
using namespace std; |
|
||||
//using namespace cv; |
|
||||
|
|
||||
@ -0,0 +1,49 @@ |
|||||
|
#ifndef _COMMANDS_H_ |
||||
|
#define _COMMANDS_H_ |
||||
|
|
||||
|
#include "SocketObject.h" |
||||
|
#include <stdlib.h> |
||||
|
|
||||
|
static struct ClientCommand* commands; |
||||
|
static unsigned int commandCount; |
||||
|
|
||||
|
// registers all commands |
||||
|
void registerCommands(); |
||||
|
|
||||
|
// print out all registered commands to the client interface |
||||
|
void printUsage(SocketObject& client, std::string& arg); |
||||
|
|
||||
|
//Handling request for current Joint Values |
||||
|
void getPositionJoints(SocketObject& client, std::string& arg); |
||||
|
//Get Position as POSE Matrix |
||||
|
void getPositionHomRowWise(SocketObject& client, std::string& arg); |
||||
|
//Get Force/torque values from TCP |
||||
|
void getForceTorqueTcp(SocketObject& client, std::string& arg); |
||||
|
//Move to given Joint combination |
||||
|
void movePTPJoints(SocketObject& client, std::string& arg); |
||||
|
//Move to given POSE position |
||||
|
void moveHomRowWiseStatus(SocketObject& client, std::string& arg); |
||||
|
//Set Velocity |
||||
|
void setSpeed(SocketObject& client, std::string& arg); |
||||
|
//Set Acceleration |
||||
|
void setAccel(SocketObject& client, std::string& arg); |
||||
|
//Starting Potential Field Movement Mode |
||||
|
void startPotFieldMode(SocketObject& client, std::string& arg); |
||||
|
//Stopping Potential Field Movement Mode |
||||
|
void stopPotFieldMode(SocketObject& client, std::string& arg); |
||||
|
//Setting Target Position HomRowWise for Potential Move |
||||
|
void setPos(SocketObject& client, std::string& arg); |
||||
|
//Setting Target Position as Joints for Potential Move |
||||
|
void setJoints(SocketObject& client, std::string& arg); |
||||
|
//Cartesian Movement |
||||
|
//Move to given POSE position |
||||
|
void moveCartesian(SocketObject& client, std::string& arg); |
||||
|
// set a specific trajectory type |
||||
|
void setTrajectoryType(SocketObject& client, std::string& arg); |
||||
|
void getTrajectoryType(SocketObject& client, std::string& arg); |
||||
|
//Quit |
||||
|
void quit(SocketObject& client, std::string& arg); |
||||
|
//DEBUGGING |
||||
|
void debug(SocketObject& client, std::string& arg); |
||||
|
|
||||
|
#endif |
||||
@ -0,0 +1,244 @@ |
|||||
|
#ifndef _LWR4_H_ |
||||
|
#define _LWR4_H_ |
||||
|
|
||||
|
#include "vec.h" |
||||
|
#include "mat.h" |
||||
|
#include "friComm.h" |
||||
|
#include "robot.h" |
||||
|
|
||||
|
|
||||
|
class LWR4 : public Robot |
||||
|
{ |
||||
|
public : |
||||
|
const static int joints = 7; |
||||
|
typedef Vec<float,joints> VecJoint; |
||||
|
|
||||
|
|
||||
|
static VecJoint getJointRange(void); |
||||
|
static VecJoint getJointVelocity(void); |
||||
|
static VecJoint getJointAcceleration(void); |
||||
|
|
||||
|
static Vec<float, 4> getDhParameter(unsigned int joint); |
||||
|
|
||||
|
static VecJoint backwardCalc(MatCarthesian pos, struct RobotConfig config ); |
||||
|
static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); |
||||
|
}; |
||||
|
Robot::VecJoint LWR4::backwardCalc(MatCarthesian M , struct RobotConfig config ) |
||||
|
{ |
||||
|
Robot::VecJoint Joints(0.0f); |
||||
|
|
||||
|
//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 d[7] = {310.4f/1000.0f, 0.0f, 400.1f/1000.0f, 0.0f, 390.0f/1000.0f, 0.0f, 78.0f/1000.0f}; |
||||
|
//Parameter |
||||
|
int ELBOW = (config.elbow ? 1.0f : -1.0f ); |
||||
|
int FLIP = (config.flip ? 1.0f : -1.0f ); |
||||
|
float J1os = (config.j1os ? 1.0f : -1.0f )/180.0*M_PI; |
||||
|
|
||||
|
//Calculating M06 |
||||
|
MatCarthesian R67; |
||||
|
int i = 6; |
||||
|
|
||||
|
R67(0,0) = cos(0); |
||||
|
R67(0,1) = -sin(0)*cos(alp[i]); |
||||
|
R67(0,2) = sin(0)*sin(alp[i]); |
||||
|
R67(0,3) = 0.0f; |
||||
|
|
||||
|
R67(1,0) = sin(0); |
||||
|
R67(1,1) = cos(0)*cos(alp[i]); |
||||
|
R67(1,2) = -cos(0)*sin(alp[i]); |
||||
|
R67(1,3) = 0.0f; |
||||
|
|
||||
|
R67(2,0) = 0.0f; |
||||
|
R67(2,1) = sin(alp[i]); |
||||
|
R67(2,2) = cos(alp[i]); |
||||
|
R67(2,3) = d[i]; |
||||
|
|
||||
|
R67(3,0) = 0.0f; |
||||
|
R67(3,1) = 0.0f; |
||||
|
R67(3,2) = 0.0f; |
||||
|
R67(3,3) = 1.0f; |
||||
|
|
||||
|
MatCarthesian M06; |
||||
|
MatCarthesian Inv; |
||||
|
Inv = R67.inv(); |
||||
|
M06=M*Inv; |
||||
|
|
||||
|
//Joint 1 including given offset |
||||
|
Joints(0) = atan2(M06(1,3),M06(0,3))+J1os; |
||||
|
|
||||
|
//Calculating M16 |
||||
|
MatCarthesian R01; |
||||
|
i = 0; |
||||
|
R01(0,0) = cos(Joints(i)); |
||||
|
R01(0,1) = -sin(Joints(i))*cos(alp[i]); |
||||
|
R01(0,2) = sin(Joints(i))*sin(alp[i]); |
||||
|
R01(0,3) = 0.0f; |
||||
|
|
||||
|
R01(1,0) = sin(Joints(i)); |
||||
|
R01(1,1) = cos(Joints(i))*cos(alp[i]); |
||||
|
R01(1,2) = -cos(Joints(i))*sin(alp[i]); |
||||
|
R01(1,3) = 0.0f; |
||||
|
|
||||
|
R01(2,0) = 0.0f; |
||||
|
R01(2,1) = sin(alp[i]); |
||||
|
R01(2,2) = cos(alp[i]); |
||||
|
R01(2,3) = d[i]; |
||||
|
|
||||
|
R01(3,0) = 0.0f; |
||||
|
R01(3,1) = 0.0f; |
||||
|
R01(3,2) = 0.0f; |
||||
|
R01(3,3) = 1.0f; |
||||
|
|
||||
|
Inv = R01.inv(); |
||||
|
MatCarthesian M16; |
||||
|
M16 = Inv*M06; |
||||
|
|
||||
|
//Joint 2 |
||||
|
float m14 = M16(0,3); |
||||
|
float m24 = M16(1,3); |
||||
|
float m34 = M16(2,3); |
||||
|
float d3 = d[2]; |
||||
|
float d5 = d[4]; |
||||
|
|
||||
|
float arg1 = (0.5*(ELBOW*m24*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14 |
||||
|
-ELBOW*m24*m24*m24+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3 |
||||
|
+2*m14*m14*m14*m14*d5*d5+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14 |
||||
|
-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14 |
||||
|
+2*m24*m24*d3*d3*m14*m14-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34 |
||||
|
+2*m14*m14*d5*d5*d3*d3+2*m24*m24*d5*d5*m14*m14))/(m24*m24+m14*m14) |
||||
|
+m34*m34+d3*d3-1*d5*d5+m14*m14+m24*m24))/(d3*m14); |
||||
|
|
||||
|
float arg2 = ELBOW*(0.5*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14-ELBOW*m24*m24*m24 |
||||
|
+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3+2*m14*m14*m14*m14*d5*d5 |
||||
|
+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24 |
||||
|
*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14+2*m24*m24*d3*d3*m14*m14 |
||||
|
-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34+2*m14*m14*d5*d5*d3*d3 |
||||
|
+2*m24*m24*d5*d5*m14*m14)))/((m24*m24+m14*m14)*d3); |
||||
|
|
||||
|
Joints(1) = atan2(arg1,arg2); |
||||
|
|
||||
|
//Calculating M26 |
||||
|
MatCarthesian R12; |
||||
|
i = 1; |
||||
|
|
||||
|
R12(0,0) = cos(Joints(i)); |
||||
|
R12(0,1) = -sin(Joints(i))*cos(alp[i]); |
||||
|
R12(0,2) = sin(Joints(i))*sin(alp[i]); |
||||
|
R12(0,3) = 0.0f; |
||||
|
|
||||
|
R12(1,0) = sin(Joints(i)); |
||||
|
R12(1,1) = cos(Joints(i))*cos(alp[i]); |
||||
|
R12(1,2) = -cos(Joints(i))*sin(alp[i]); |
||||
|
R12(1,3) = 0.0f; |
||||
|
|
||||
|
R12(2,0) = 0.0f; |
||||
|
R12(2,1) = sin(alp[i]); |
||||
|
R12(2,2) = cos(alp[i]); |
||||
|
R12(2,3) = d[i]; |
||||
|
|
||||
|
R12(3,0) = 0.0f; |
||||
|
R12(3,1) = 0.0f; |
||||
|
R12(3,2) = 0.0f; |
||||
|
R12(3,3) = 1.0f; |
||||
|
|
||||
|
Mat4f R02; |
||||
|
R02 = R01*R12; |
||||
|
Inv = R02.inv(); |
||||
|
Mat4f M26; |
||||
|
M26 = Inv*M06; |
||||
|
|
||||
|
//Joint 3 |
||||
|
Joints(2) = atan2(ELBOW*M26(1,3),ELBOW*M26(0,3)); |
||||
|
|
||||
|
//Calculating M36 |
||||
|
MatCarthesian R23; |
||||
|
i = 2; |
||||
|
|
||||
|
R23(0,0) = cos(Joints(i)); |
||||
|
R23(0,1) = -sin(Joints(i))*cos(alp[i]); |
||||
|
R23(0,2) = sin(Joints(i))*sin(alp[i]); |
||||
|
R23(0,3) = 0.0f; |
||||
|
|
||||
|
R23(1,0) = sin(Joints(i)); |
||||
|
R23(1,1) = cos(Joints(i))*cos(alp[i]); |
||||
|
R23(1,2) = -cos(Joints(i))*sin(alp[i]); |
||||
|
R23(1,3) = 0.0f; |
||||
|
|
||||
|
R23(2,0) = 0.0f; |
||||
|
R23(2,1) = sin(alp[i]); |
||||
|
R23(2,2) = cos(alp[i]); |
||||
|
R23(2,3) = d[i]; |
||||
|
|
||||
|
R23(3,0) = 0.0f; |
||||
|
R23(3,1) = 0.0f; |
||||
|
R23(3,2) = 0.0f; |
||||
|
R23(3,3) = 1.0f; |
||||
|
|
||||
|
MatCarthesian R03; |
||||
|
R03 = R01*R12*R23; |
||||
|
Inv = R03.inv(); |
||||
|
MatCarthesian M36; |
||||
|
M36 = Inv*M06; |
||||
|
|
||||
|
//Joint 4 |
||||
|
Joints(3) = atan2(M36(0,3),-M36(1,3)); |
||||
|
|
||||
|
//Calculating M47 |
||||
|
MatCarthesian R34; |
||||
|
i = 3; |
||||
|
R34(0,0) = cos(Joints(i)); |
||||
|
R34(0,1) = -sin(Joints(i))*cos(alp[i]); |
||||
|
R34(0,2) = sin(Joints(i))*sin(alp[i]); |
||||
|
R34(0,3) = 0.0f; |
||||
|
|
||||
|
R34(1,0) = sin(Joints(i)); |
||||
|
R34(1,1) = cos(Joints(i))*cos(alp[i]); |
||||
|
R34(1,2) = -cos(Joints(i))*sin(alp[i]); |
||||
|
R34(1,3) = 0.0f; |
||||
|
|
||||
|
R34(2,0) = 0.0f; |
||||
|
R34(2,1) = sin(alp[i]); |
||||
|
R34(2,2) = cos(alp[i]); |
||||
|
R34(2,3) = d[i]; |
||||
|
|
||||
|
R34(3,0) = 0.0f; |
||||
|
R34(3,1) = 0.0f; |
||||
|
R34(3,2) = 0.0f; |
||||
|
R34(3,3) = 1.0f; |
||||
|
|
||||
|
MatCarthesian R04; |
||||
|
R04 = R01*R12*R23*R34; |
||||
|
Inv = R04.inv(); |
||||
|
MatCarthesian M47; |
||||
|
M47 = Inv*M; |
||||
|
|
||||
|
//Joint 5 |
||||
|
float th = 0.0001; |
||||
|
if (abs(M47(1,2))<th && abs(M47(0,2))<th) |
||||
|
Joints(4) = 0.0f; |
||||
|
else |
||||
|
Joints(4) = atan2(FLIP*M47(1,2),FLIP*M47(0,2)); |
||||
|
|
||||
|
//Joint 6 |
||||
|
Joints(5) = atan2(FLIP*sqrt(1-M47(2,2)*M47(2,2)),M47(2,2)); |
||||
|
|
||||
|
//Joint 7 |
||||
|
if (abs(M47(2,1))<th && abs(M47(2,0))<th) |
||||
|
Joints(6) = 0.0f; |
||||
|
else |
||||
|
Joints(6) = atan2(FLIP*M47(2,1),FLIP*-M47(2,0)); |
||||
|
|
||||
|
for (int i = 0; i<7; i++) |
||||
|
{ |
||||
|
Joints(i) = Joints(i)*180.0/M_PI; |
||||
|
} |
||||
|
|
||||
|
//Kuka Joints |
||||
|
//Joints[3] = -Joints[3]; |
||||
|
Joints(1) = -Joints(1); |
||||
|
Joints(5) = -Joints(5); |
||||
|
|
||||
|
return Joints; |
||||
|
} |
||||
|
#endif |
||||
@ -0,0 +1,96 @@ |
|||||
|
#ifndef _ROBOT_H_ |
||||
|
#define _ROBOT_H_ |
||||
|
|
||||
|
#include "vec.h" |
||||
|
#include "mat.h" |
||||
|
#include "friComm.h" |
||||
|
|
||||
|
typedef Mat<float, 4> MatCarthesian; |
||||
|
typedef Vec<float, FRI_CART_VEC> VecTorque; |
||||
|
|
||||
|
class Robot |
||||
|
{ |
||||
|
public : |
||||
|
const static int joints = 7; |
||||
|
typedef Vec<float,joints> VecJoint; |
||||
|
|
||||
|
static int getJointCount(void) |
||||
|
{ |
||||
|
} |
||||
|
static VecJoint getJointRange(void) |
||||
|
{ |
||||
|
return VecJoint(180.0f); |
||||
|
} |
||||
|
static VecJoint getJointVelocity(void) |
||||
|
{ |
||||
|
return VecJoint(200.0f); |
||||
|
} |
||||
|
static VecJoint getJointAcceleration(void) |
||||
|
{ |
||||
|
return VecJoint(10.0f); |
||||
|
} |
||||
|
|
||||
|
static Vec<float, 4> getDhParameter(unsigned int joint) |
||||
|
{ |
||||
|
// unused parameter |
||||
|
(void) joint; |
||||
|
return Vec<float,4>(0.0f); |
||||
|
} |
||||
|
|
||||
|
struct RobotConfig |
||||
|
{ |
||||
|
bool elbow; |
||||
|
bool flip; |
||||
|
bool j1os; |
||||
|
}; |
||||
|
|
||||
|
static VecJoint backwardCalc(MatCarthesian, struct RobotConfig config ); |
||||
|
static MatCarthesian forwardCalc(VecJoint, struct RobotConfig config ); |
||||
|
}; |
||||
|
|
||||
|
|
||||
|
|
||||
|
#if 0 |
||||
|
Mat4f vecToMat(float vec[12]) |
||||
|
{ |
||||
|
Mat4f result; |
||||
|
for (int i=0; i<3; i++) // ZEILE |
||||
|
{ |
||||
|
for (int j=0; j<4; j++) // SPALTE |
||||
|
{ |
||||
|
result(i,j) = vec[i*4+j]; |
||||
|
} |
||||
|
} |
||||
|
result(3,3)= 1.0f; |
||||
|
return result; |
||||
|
} |
||||
|
|
||||
|
float* matToVec(Mat4f mat) |
||||
|
{ |
||||
|
float* vec = new float[12]; |
||||
|
for (int j=0;j<3;j++) |
||||
|
{ |
||||
|
for (int k=0;k<4;k++) |
||||
|
{ |
||||
|
vec[j*4+k] = mat(j,k); |
||||
|
} |
||||
|
} |
||||
|
return vec; |
||||
|
} |
||||
|
|
||||
|
Mat4f getTranslation(double tx, double ty, double tz) |
||||
|
{ |
||||
|
Mat4f transl; |
||||
|
transl(0,0) = 1; |
||||
|
transl(1,1) = 1; |
||||
|
transl(2,2) = 1; |
||||
|
|
||||
|
transl(0,3) = tx; |
||||
|
transl(1,3) = ty; |
||||
|
transl(2,3) = tz; |
||||
|
transl(3,3) = 1; |
||||
|
return transl; |
||||
|
} |
||||
|
#endif |
||||
|
|
||||
|
#endif |
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue