void SpencerRobotReader::updateRobot(tf::TransformListener &listener) {
Robot* curRobot = lastConfig_["spencer"];
Joint* curJoint = new Joint("spencer_base_link", "spencer");
curJoint->setName(spencerJointsName_[0]);
// We start with base:
setRobotJointLocation(listener, curJoint);
curRobot->setOrientation(curJoint->getOrientation());
curRobot->setPosition(curJoint->getPosition());
curRobot->setTime(curJoint->getTime());
//printf("spencer robot: %f, %f, %f\n", curRobot->getPosition().get<0>(), curRobot->getPosition().get<1>(), curRobot->getPosition().get<2>());
delete curJoint;
}
int main(int argc, char* argv[])
{
RobotSimulator * simulator = new RobotSimulator();
simulator->simStart();
Joint * joint = new Joint(simulator,(char*)"joint7",45*RAD,-90*RAD);
if (simulator->clientIdStatus())
{
simulator->simStartSimulation(simx_opmode_oneshot_wait);
while (simulator->simGetConnectionId() != -1)
{
double position;
cout << "Ingrese un valor para el angulo del motor #1 en grados ó -1 para terminar:" << endl;
cin >> position;
if(position == 999) break;
else
{
clog << "joint_pos: " << joint->getJointCurrentPosition((char *)"RAD") << endl;
double * pos;
double * ori;
pos = joint->getPosition(-1);
ori = joint->getOrientation(-1);
clog << "pos: " << pos[0] << " - " << pos[1] << " - " << pos[2] << endl;
clog << "ori: " << ori[0] << " - " << ori[1] << " - " << ori[2] << endl;
clog << "moving to position: " << position << endl;
joint->setJointPosition(position,(char *)"DEG");
}
sleep(2);
}
simulator->simStopSimulation(simx_opmode_oneshot_wait);
}