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