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::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());
}
}