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