Esempio n. 1
0
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;	
}
Esempio n. 2
0
  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);
  }