void WorldRpcInterface::setRot( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
if ((command.size() - n) == 3) {
double x = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << x << std::endl; // x position
double y = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << y << std::endl; // y position
double z = command.get(n).asDouble()*M_PI/180.; n++; //std::cout << z << std::endl; // z position
object->setCartesianOrientation( QVector3D(x,y,z) );
reply.addString("Set rotation (about x,y,z in degrees).");
} else {
// replace rotation part of object's T matrix
QMatrix4x4 rt = object->getT();
for (int i = 0; i<3; i++) {
for (int j = 0; j<3; j++) {
rt(i, j) = command.get(n).asDouble(); n++;
}
}
object->setT(rt);
reply.addString("Set full rotation matrix.");
}
}
}
void WorldRpcInterface::removeObject( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
object->kill();
reply.addString("Removed object");
}
}
void WorldRpcInterface::setRTfromSim(const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n) {
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object ) {
QMatrix4x4 rt;
std::string name = object->getName().toStdString();
if(parseSimRTBottle(name, command, n, rt)) {
object->setT(rt);
} else {
reply.addString("parseSimRTBottle: invalid number of arguments supplied");
}
}
}
void WorldRpcInterface::set( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
double x = command.get(n).asDouble(); n++; //std::cout << x << std::endl; // x position
double y = command.get(n).asDouble(); n++; //std::cout << y << std::endl; // y position
double z = command.get(n).asDouble(); n++; //std::cout << z << std::endl; // z position
object->setPosition( QVector3D(x,y,z) );
reply.addString("Set Cartesian position of object.");
}
}
void WorldRpcInterface::getState( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
const qreal* T = object->getT().data();
for ( int i = 0; i < 16; i++ )
{
reply.addDouble( T[i] );
}
//reply.addString("Set rotation (about x,y,z in degrees) of object.");
}
}
KinematicModel::CompositeObject* ObjectSoup::makeARandomObjectLikeAMothaFucka()
{
KinematicModel::CompositeObject* obj = new KinematicModel::CompositeObject( theModel.OBSTACLE(), theModel.OBSTACLE() );
// choose a random color for the object
QColor collidingColor = QColor( qrand()%255, qrand()%255, qrand()%255, 0.5 );
QColor freeColor = collidingColor;
freeColor.setAlphaF( 1.0 );
obj->setCollidingColor(collidingColor);
obj->setFreeColor(freeColor);
// choose a random number of primitives to append to the object 1 to 5 inclusive
int numPrimitives = (qrand() % 6 + 1);
if (verbose) printf( " Num Primitives: %d\n", numPrimitives);
KinematicModel::PrimitiveObject* primitive;
for ( int j=0; j<numPrimitives; ++j )
{
// choose a random geometry type for each primitive
KinematicModel::GeomType geomType = (KinematicModel::GeomType)( qrand() % ((int)KinematicModel::BOX+1) );
if (verbose) printf( " Geometry Type: %d\n", (int)geomType );
switch (geomType)
{
case KinematicModel::SPHERE: primitive = new KinematicModel::Sphere( (float)qrand()/RAND_MAX/10.0+0.01 );
break;
case KinematicModel::CYLINDER: primitive = new KinematicModel::Cylinder( (float)qrand()/RAND_MAX/10+.01,
(float)2*qrand()/RAND_MAX/10+.01 );
break;
case KinematicModel::BOX: primitive = new KinematicModel::Box( QVector3D((float)qrand()/RAND_MAX/10+.01,
(float)qrand()/RAND_MAX/10+.01,
(float)qrand()/RAND_MAX/10+.01) );
break;
default:
break;
}
primitive->setCollidingColor( collidingColor );
primitive->setFreeColor(freeColor);
primitive->translate(randomTranslation(0.1));
primitive->cartesianRotate(randomRotation());
obj->appendPrimitive(primitive);
}
obj->translate(randomTranslation(1.0));
primitive->cartesianRotate(randomRotation());
theModel.appendObject(obj);
return obj;
}
void WorldRpcInterface::respClass( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
KinematicModel::CompositeObject* object = getObject( command, reply, n );
if ( object )
{
//model->clearWorldObject(object);
model->removeWorldObject(object);
int type = command.get(n).asVocab();
QColor collidingColor,freeColor;
switch (type)
{
case VOCAB_OBSTACLE:
freeColor = Qt::blue;
freeColor = freeColor.lighter();
collidingColor = freeColor;
freeColor.setAlphaF(0.5);
collidingColor.setAlphaF(0.5);
object->setResponseClass(model->OBSTACLE());
object->setFreeColor( freeColor );
object->setCollidingColor( collidingColor );
reply.addString("Changed object type to 'obstacle'.");
break;
case VOCAB_TARGET:
freeColor = Qt::green;
freeColor = freeColor.lighter();
collidingColor = freeColor;
freeColor.setAlphaF(1.0);
collidingColor.setAlphaF(0.5);
object->setResponseClass(model->TARGET());
object->setFreeColor( freeColor );
object->setCollidingColor( collidingColor );
reply.addString("Changed object type to 'target'.");
break;
default:
reply.addString("Unknown definition, use 'obs' or 'tgt'.");
}
model->appendObject( object );
}
}
void WorldRpcInterface::make( const yarp::os::Bottle& command, yarp::os::Bottle& reply, int& n )
{
int geom = command.get(n).asVocab();
n++;
try {
KinematicModel::CompositeObject* composite = new KinematicModel::CompositeObject( model->OBSTACLE(), model->FIELD() );
KinematicModel::PrimitiveObject* primitive;
if ( geom == VOCAB_SPH || geom == VOCAB_SSPH ) {
double r = command.get(n).asDouble(); n++; // radius
double px = command.get(n).asDouble(); n++; // x position
double py = command.get(n).asDouble(); n++; // y position
double pz = command.get(n).asDouble(); n++; // z position
primitive = new KinematicModel::Sphere( r );
composite->appendPrimitive( primitive );
composite->setPosition( QVector3D(px,py,pz) );
QString name;
if ( geom == VOCAB_SPH ) { name = "sph" + QString::number(++s); }
else if ( geom == VOCAB_SSPH ) { name = "ssph" + QString::number(++ss); }
composite->setName(name);
//printf("%s\n",name.toLocal8Bit().data());
reply.addString( name.toLocal8Bit().data() );
} else if ( geom == VOCAB_CYL || geom == VOCAB_SCYL ) {
double r = command.get(n).asDouble(); n++; // radius
double h = command.get(n).asDouble(); n++; // height
double px = command.get(n).asDouble(); n++; // x position
double py = command.get(n).asDouble(); n++; // y position
double pz = command.get(n).asDouble(); n++; // z position
//if ( geom == VOCAB_CYL ) { world->newCylinder( r, h, QVector3D(px,py,pz) ); }
//else if ( geom == VOCAB_SCYL ) { world->newSCylinder( r, h, QVector3D(px,py,pz) ); }
primitive = new KinematicModel::Cylinder( r, h );
composite->appendPrimitive( primitive );
composite->setPosition( QVector3D(px,py,pz) );
QString name;
if ( geom == VOCAB_CYL ) { name = "cyl" + QString::number(++c); }
else if ( geom == VOCAB_SCYL ) { name = "scyl" + QString::number(++sc); }
composite->setName(name);
reply.addString( name.toLocal8Bit().data() );
} else if ( geom == VOCAB_BOX || geom == VOCAB_SBOX ) {
double x = command.get(n).asDouble(); n++; // x size
double y = command.get(n).asDouble(); n++; // y size
double z = command.get(n).asDouble(); n++; // z size
double px = command.get(n).asDouble(); n++; // x position
double py = command.get(n).asDouble(); n++; // y position
double pz = command.get(n).asDouble(); n++; // z position
//if ( geom == VOCAB_BOX ) { world->newBox( QVector3D(x,y,z), QVector3D(px,py,pz) ); }
//else if ( geom == VOCAB_SBOX ) { world->newSBox( QVector3D(x,y,z), QVector3D(px,py,pz) ); }
primitive = new KinematicModel::Box( QVector3D(x,y,z) );
composite->appendPrimitive( primitive );
composite->setPosition( QVector3D(px,py,pz) );
QString name;
if ( geom == VOCAB_BOX ) { name = "box" + QString::number(++b); }
else if ( geom == VOCAB_SBOX ) { name = "sbox" + QString::number(++sb); }
composite->setName(name);
reply.addString( name.toLocal8Bit().data() );
}
else { reply.addString("MK ERROR: Unknown geometry... use '(s)sph', '(s)cyl', or '(s)box'"); }
model->appendObject( composite );
}
catch (std::exception& e)
{
reply.clear();
reply.addString(e.what());
}
}
