Esempio n. 1
0
double RobotVREP::getJointForce(Joint * joint, simxInt operationMode)
{
	int uniqueID = joint->getUniqueObjectId();
	int vrepHandlerVectorPosition =jointIdToVrepHandler_map.at ( uniqueID );
	int * handler = VrepHandlerVector.at( vrepHandlerVectorPosition );

	float force;

	int error = simxGetJointForce(clientID, *handler, &force, operationMode);
	if(error != 0) vrep_error << "simxGetJointForce - " << *handler << " : "<< error << endl;

	return (double)force;
}
Esempio n. 2
0
 void YouBotBaseService::readJointStates()
  {
    float p,v,e;

    for(int i = 0; i < NR_OF_BASE_SLAVES; ++i)
    {                
      simxGetJointPosition(clientID, vrep_joint_handle[i], &p, simx_opmode_buffer);
      simxGetObjectFloatParameter(clientID, vrep_joint_handle[i], 2012, &v, simx_opmode_buffer);
      simxGetJointForce(clientID, vrep_joint_handle[i], &e, simx_opmode_buffer);                  
 
      m_joint_state.position[i] = p;
      m_joint_state.velocity[i] = v;
      m_joint_state.effort[i] = e;
    }
    
    joint_state.write(m_joint_state);    
  }
Esempio n. 3
0
  YouBotBaseService::YouBotBaseService(const string& name, TaskContext* parent, long i_clientID):
  Hilas::IRobotBaseService(name, parent, YouBot::NR_OF_BASE_SLAVES, 15, YouBot::JOINT_BASE_NAME_ARRAY,i_clientID), m_min_slave_nr(1)
  {
    vrep_joint_handle.assign(YouBot::NR_OF_BASE_SLAVES,0);
    joint_base_position_prev.assign(4,0.0);
    odom_wheelPositions.assign(4,0.0*radian);

    is_in_visualization_mode = false;
    
    simxGetObjectHandle(clientID,"youBot",&all_robot_handle, simx_opmode_oneshot_wait);

    for (int i = 0; i < YouBot::NR_OF_BASE_SLAVES; ++i)
    {
      simxGetObjectHandle(clientID, YouBot::JOINT_BASE_NAME_ARRAY[i].c_str(), &vrep_joint_handle[i], simx_opmode_oneshot_wait);
    }

    configfile.reset(new youbot::ConfigFile("youbot-base.cfg",CFG_YOUBOT_BASE));
    configfile->readInto(kinematicConfig.rotationRatio, "YouBotKinematic", "RotationRatio");   
    configfile->readInto(kinematicConfig.slideRatio, "YouBotKinematic", "SlideRatio");

    double dummy = 0;
    
    configfile->readInto(dummy, "YouBotKinematic", "LengthBetweenFrontAndRearWheels_[meter]");
    kinematicConfig.lengthBetweenFrontAndRearWheels = dummy * meter;
    configfile->readInto(dummy, "YouBotKinematic", "LengthBetweenFrontWheels_[meter]");
    kinematicConfig.lengthBetweenFrontWheels = dummy * meter;
    configfile->readInto(dummy, "YouBotKinematic", "WheelRadius_[meter]");
    kinematicConfig.wheelRadius = dummy * meter;
    youBotBaseKinematic.setConfiguration(kinematicConfig);

    // open data streaming from the simulator server
    float dummy_var;

    for(int i = 0; i < YouBot::NR_OF_BASE_SLAVES; ++i)
    {
      simxGetJointPosition(clientID, vrep_joint_handle[i], &dummy_var, simx_opmode_streaming);
      simxGetObjectFloatParameter(clientID, vrep_joint_handle[i], 2012, &dummy_var, simx_opmode_streaming);
      simxGetJointForce(clientID, vrep_joint_handle[i], &dummy_var, simx_opmode_streaming); 
    }    

  }