Ejemplo n.º 1
0
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;


}
Ejemplo n.º 2
0
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);
	}