void RobotVREP::setJointTargetPosition(Joint * joint, double joint_pos, simxInt operationMode)
{
int uniqueID = joint->getUniqueObjectId();
int vrepHandlerVectorPosition =jointIdToVrepHandler_map.at ( uniqueID );
int * handler = VrepHandlerVector.at( vrepHandlerVectorPosition );
int error = simxSetJointTargetPosition(clientID, *handler, (float)joint_pos, operationMode);
if(error != 0) vrep_error << "simxSetJointTargetPosition - " << *handler << " : "<< error << endl;
}
void YouBotBaseService::setJointSetpoints()
{
joint_position_command.read(m_joint_position_command);
joint_velocity_command.read(m_joint_velocity_command);
joint_effort_command.read(m_joint_effort_command);
FlowStatus f = joint_position_command.read(m_joint_position_command);
FlowStatus f1 = joint_velocity_command.read(m_joint_velocity_command);
FlowStatus f2 = joint_effort_command.read(m_joint_effort_command);
simxPauseCommunication(clientID,1);
for (unsigned int joint_nr = 0; joint_nr < YouBot::NR_OF_BASE_SLAVES; ++joint_nr)
{
switch (m_joint_ctrl_modes[joint_nr])
{
case(Hilas::PLANE_ANGLE):
{
//if(f != NewData) break;
simxSetJointTargetPosition(clientID,vrep_joint_handle[joint_nr],m_joint_position_command.positions[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::ANGULAR_VELOCITY):
{
//if(f1 != NewData) break;
// Little hack
int sign[4] = {1,-1,1,-1};
simxSetJointTargetVelocity(clientID,vrep_joint_handle[joint_nr],m_joint_velocity_command.velocities[joint_nr] * sign[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::TORQUE):
{
//if(f2 != NewData) break;
simxSetJointForce(clientID,vrep_joint_handle[joint_nr],m_joint_effort_command.efforts[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::MOTOR_STOP):
{
simxSetJointTargetPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
break;
}
case(Hilas::TWIST):
{
log(Error) << "Cannot be in TWIST ctrl_mode (programming error)"
<< endlog();
this->getOwner()->error();
break;
}
default:
{
log(Error) << "Case not recognized." << endlog();
this->getOwner()->error();
break;
}
}
}
simxPauseCommunication(clientID,0);
}