/*!
This is the main routine for parsing input on the clientSocket.
There should be one command for each line of input. This reads one
line, and looks at the first word (up to the first space character) to
determine the command. Then if there are body or robot indices to read,
it calls a support routine to read those and return a vector of bodies or
robots. These are then passed to the appropriate routine to carry out the
action and write out any necessary results.
*/
void
ClientSocket::readClient()
{
int i,numData,numBodies,numRobots;
double time;
std::vector<Body *> bodyVec;
std::vector<Robot *> robVec;
bool ok;
while ( canReadLine() ) {
line = readLine();
line.truncate(line.length()-1); //strip newline character
lineStrList =
QStringList::split(' ',line);
strPtr = lineStrList.begin();
#ifdef GRASPITDBG
std::cout <<"Command parser line: "<<line << std::endl;
#endif
if (*strPtr == "getContacts") {
strPtr++; if (strPtr == lineStrList.end()) continue;
numData = (*strPtr).toInt(&ok); strPtr++;
if (!ok) continue;
#ifdef GRASPITDBG
std::cout << "Num data: "<<numData<<std::endl;
#endif
if (readBodyIndList(bodyVec)) continue;
numBodies = bodyVec.size();
for (i=0;i<numBodies;i++)
sendContacts(bodyVec[i],numData);
}
else if (*strPtr == "getAverageContacts") {
strPtr++;
if (readBodyIndList(bodyVec)) continue;
numBodies = bodyVec.size();
for (i=0;i<numBodies;i++)
sendAverageContacts(bodyVec[i]);
}
else if (*strPtr == "getBodyName") {
strPtr++;
if (readBodyIndList(bodyVec)) continue;
numBodies = bodyVec.size();
for (i=0;i<numBodies;i++)
sendBodyName(bodyVec[i]);
}
else if(*strPtr == "setBodyName") {
strPtr++;
int body_index;
if(strPtr != lineStrList.end()){
body_index = strPtr->toInt(&ok);
strPtr++;
if(strPtr == lineStrList.end())
return;
if (body_index == -1 || body_index >= graspItGUI->getIVmgr()->getWorld()->getNumBodies())
{
body_index = graspItGUI->getIVmgr()->getWorld()->getNumBodies() - 1;
}
graspItGUI->getIVmgr()->getWorld()->getBody(body_index)->setName(*strPtr);
}
}
else if (*strPtr == "getRobotName") {
strPtr++;
if (readRobotIndList(robVec)) continue;
numRobots = robVec.size();
for (i=0;i<numRobots;i++)
sendRobotName(robVec[i]);
}
else if (*strPtr == "getDOFVals") {
strPtr++;
if (readRobotIndList(robVec)) continue;
numRobots = robVec.size();
for (i=0;i<numRobots;i++)
sendDOFVals(robVec[i]);
}
else if (*strPtr == "moveDOFs") {
strPtr++;
readDOFVals();
}
else if (*strPtr == "render")
graspItGUI->getIVmgr()->getViewer()->render();
else if (*strPtr == "setDOFForces") {
strPtr++;
if (readRobotIndList(robVec)) continue;
numRobots = robVec.size();
for (i=0;i<numRobots;i++)
if (readDOFForces(robVec[i])==FAILURE) continue;
}
else if ((*strPtr) == "moveDynamicBodies") {
strPtr++;
if (strPtr == lineStrList.end()) ok = FALSE;
else {
time = (*strPtr).toDouble(&ok); strPtr++;
}
if (!ok)
moveDynamicBodies(-1);
else
moveDynamicBodies(time);
}
else if (*strPtr == "computeNewVelocities") {
#ifdef GRASPITDBG
std::cout << "cnv" << std::endl;
#endif
strPtr++; if (strPtr == lineStrList.end()) continue;
time = (*strPtr).toDouble(&ok); strPtr++;
if (!ok) continue;
#ifdef GRASPITDBG
std::cout << time <<std::endl;
#endif
computeNewVelocities(time);
}
else if ((*strPtr) == "outputPlannerResults"){
strPtr++;
outputPlannerResults(0);
}
else if ((*strPtr) == "outputCurrentGrasp"){
strPtr++;
outputCurrentGrasp();
}
else if ((*strPtr) == "sendBodyTransf"){
strPtr++;
verifyInput(1);
sendBodyTransf();
}
else if ((*strPtr) == "setBodyTransf"){
strPtr++;
verifyInput(7);
setBodyTransf();
}
else if ((*strPtr) == "addObstacle"){
strPtr++;
verifyInput(1);
addObstacle(*(strPtr+1));
strPtr+=2;
}
else if ((*strPtr) == "addObject"){
verifyInput(2);
addGraspableBody(*(strPtr+1), *(strPtr+2));
strPtr+=3;
verifyInput(7);
transf object_pose;
readTransf(&object_pose);
World * w = graspItGUI->getIVmgr()->getWorld();
w->getGB(w->getNumGB() - 1)->setTran(object_pose);
}
else if ((*strPtr) == "getCurrentHandTran"){
strPtr++;
getCurrentHandTran();
}
else if ((*strPtr) == "signalGraspUnreachable"){
strPtr+=4;
std::cout << line.toStdString() << std::endl;
graspItGUI->getIVmgr()->blinkBackground();
}
else if ((*strPtr) == "getPlannerTarget"){
strPtr+=1;
QTextStream os (this) ;
os << graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject()->getName() << "\n";
}
else if ((*strPtr) == "setPlannerTarget"){
QTextStream os(this);
os << setPlannerTarget(*(strPtr+1)) << "\n";
strPtr+=2;
}
else if ((*strPtr) == "rotateHandLat"){
strPtr+=1;
rotateHandLat();
}
else if ((*strPtr) == "rotateHandLong"){
strPtr+=1;
rotateHandLong();
}
else if ((*strPtr) == "exec"){
strPtr+=1;
exec();
}
else if ((*strPtr) == "next"){
strPtr+=1;
next();
}
else if ((*strPtr) == "addPointCloud")
{
strPtr += 1;
addPointCloud();
//QTextStream os(this);
//os << addPointCloud() <<" \n";
}
else if ((*strPtr) == "setCameraOrigin")
{
strPtr += 1;
setCameraOrigin();
}
else if ((*strPtr) == "removeBodies"){
strPtr += 1;
removeBodies();
}
else if ((*strPtr) == "clearGraspableBodies"){
strPtr += 1;
removeBodies(true);
}
}
}
void BulletCHOP::execute(const CHOP_Output* output,
const CHOP_InputArrays* inputs,
void* reserved)
{
myExecuteCount++;
dynamicsWorld->setGravity(btVector3(inputs->floatInputs[1].values[0],inputs->floatInputs[1].values[1],inputs->floatInputs[1].values[2]));
// In this case we'll just take the first input and re-output it with it's
// value divivded by two
if (inputs->numCHOPInputs > 0)
{
if(inputs->floatInputs[0].values[0] == 1) {
removeBodies();
for (int i = 0; i < inputs->CHOPInputs[0].length; i++){
output->channels[0][i] = inputs->CHOPInputs[0].channels[0][i];
output->channels[1][i] = inputs->CHOPInputs[0].channels[1][i];
output->channels[2][i] = inputs->CHOPInputs[0].channels[2][i];
output->channels[3][i] = inputs->CHOPInputs[0].channels[3][i];
output->channels[4][i] = inputs->CHOPInputs[0].channels[4][i];
output->channels[5][i] = inputs->CHOPInputs[0].channels[5][i];
output->channels[6][i] = inputs->CHOPInputs[0].channels[6][i];
output->channels[7][i] = inputs->CHOPInputs[0].channels[7][i];
output->channels[8][i] = inputs->CHOPInputs[0].channels[8][i];
output->channels[9][i] = 0;
//output->channels[10][i] = 0;
//output->channels[11][i] = 0;
btVector3 pos = btVector3(
inputs->CHOPInputs[0].channels[0][i],
inputs->CHOPInputs[0].channels[1][i],
inputs->CHOPInputs[0].channels[2][i]);
btVector3 rot = 0.017453*btVector3(
inputs->CHOPInputs[0].channels[3][i],
inputs->CHOPInputs[0].channels[4][i],
inputs->CHOPInputs[0].channels[5][i]);
btVector3 scale = btVector3(
inputs->CHOPInputs[0].channels[6][i],
inputs->CHOPInputs[0].channels[7][i],
inputs->CHOPInputs[0].channels[8][i]);
addBody(pos, rot, scale, 1);
}
for (int i = 0; i < inputs->CHOPInputs[1].length; i++){
btVector3 pos = btVector3(
inputs->CHOPInputs[1].channels[0][i],
inputs->CHOPInputs[1].channels[1][i],
inputs->CHOPInputs[1].channels[2][i]);
btVector3 rot = 0.017453*btVector3(
inputs->CHOPInputs[1].channels[3][i],
inputs->CHOPInputs[1].channels[4][i],
inputs->CHOPInputs[1].channels[5][i]);
btVector3 scale = btVector3(
inputs->CHOPInputs[1].channels[6][i],
inputs->CHOPInputs[1].channels[7][i],
inputs->CHOPInputs[1].channels[8][i]);
addBody(pos, rot, scale, 0);
}
//addPlane(btVector3(0,0,0),btVector3(0,0,0));
} else {
//dynamicsWorld->stepSimulation(1.f/60.f,10);
ms = getDeltaTimeMicroseconds() / 1000000.f;
//ms = 1.4;
dynamicsWorld->stepSimulation(ms,inputs->floatInputs[0].values[2],1.f/inputs->floatInputs[0].values[1]);
//dynamicsWorld->stepSimulation(ms,10);
//dynamicsWorld->stepSimulation(ms,inputs->floatInputs[0].values[2]);
//dynamicsWorld->stepSimulation(1.0f/60.0f,10);
if (dynamicsWorld->getNumCollisionObjects()!=0) {
for (int i = 0; i < inputs->CHOPInputs[0].length; i++){
btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
btTransform trans;
body->getMotionState()->getWorldTransform(trans);
output->channels[0][i] = float(trans.getOrigin().getX());
output->channels[1][i] = float(trans.getOrigin().getY());
output->channels[2][i] = float(trans.getOrigin().getZ());
btScalar rotX, rotY, rotZ;
trans.getBasis().getEulerZYX(rotX,rotY,rotZ);
output->channels[3][i] = float(rotX)*57.2958;
output->channels[4][i] = float(rotY)*57.2958;
output->channels[5][i] = float(rotZ)*57.2958;
output->channels[6][i] = inputs->CHOPInputs[0].channels[6][i];
output->channels[7][i] = inputs->CHOPInputs[0].channels[7][i];
output->channels[8][i] = inputs->CHOPInputs[0].channels[8][i];
btVector3 vel = body->getLinearVelocity();
output->channels[9][i] = pow(vel.x(),2)+pow(vel.y(),2)+pow(vel.z(),2);
//output->channels[10][i] = 0;
//output->channels[11][i] = 0;
}
for (int i = 0; i < inputs->CHOPInputs[1].length; i++){
btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i+inputs->CHOPInputs[0].length];
btRigidBody* body = btRigidBody::upcast(obj);
btVector3 pos = btVector3(
inputs->CHOPInputs[1].channels[0][i],
inputs->CHOPInputs[1].channels[1][i],
inputs->CHOPInputs[1].channels[2][i]);
btVector3 rot = 0.017453*btVector3(
inputs->CHOPInputs[1].channels[3][i],
inputs->CHOPInputs[1].channels[4][i],
inputs->CHOPInputs[1].channels[5][i]);
btTransform trans;
trans.setOrigin(pos);
btMatrix3x3 rotMat;
rotMat.setEulerZYX(rot.x(),rot.y(),rot.z());
trans.setBasis(rotMat);
body->getMotionState()->setWorldTransform(trans);
}
}
}
}
else // If not input is connected, lets output a sine wave instead
{
}
}
