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