You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
594 lines
14 KiB
594 lines
14 KiB
#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;
|
|
}
|
|
|
|
Trajectory<JOINT_NUMBER>* newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
|
|
|
|
// add trajectory to queue for sender thread
|
|
SvrData::getInstance()->pushTrajectory(newTrajectory);
|
|
|
|
SvrData::getInstance()->setCommandedJointPos(newJointPos);
|
|
|
|
//Wait to end of movement
|
|
float sampleTimeMs = SvrData::getInstance()->getSampleTimeMs();
|
|
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);
|
|
//}
|
|
}
|