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