Example #1
0
void RobotVREP::setObjectOrientation(Object * object, vector < double > orientation, int relativeTo, simxInt operationMode)
{
	int uniqueId = object->getUniqueObjectId();
	int vrepHandlerVectorPosition = objectIdToVrepHandler_map.at(uniqueId);
	int * handler = VrepHandlerVector.at(vrepHandlerVectorPosition);
	float * ori = new float[3];

	ori[0] = (float)orientation.at(0);
	ori[1] = (float)orientation.at(1);
	ori[2] = (float)orientation.at(2);

	int error = simxSetObjectOrientation(clientID, *handler, relativeTo, ori, operationMode);
	if(error != 0) vrep_error << "simxSetObjectOrientation: " << *handler << " : " << error << endl;

	delete[] ori;
}
Example #2
0
  void YouBotBaseService::update()
  {
    if(is_in_visualization_mode)
    {
      in_odometry_state.read(m_odometry_state);

      simxFloat pos[3];
      pos[0] = m_odometry_state.pose.pose.position.x;
      pos[1] = m_odometry_state.pose.pose.position.y;
      pos[2] = m_odometry_state.pose.pose.position.z;

      simxSetObjectPosition(clientID,all_robot_handle,-1,pos,simx_opmode_oneshot);

      double euler[3];
      simxFloat euler_s[3];

      //different coordinate systems, fixing it here
      KDL::Rotation orientation  = KDL::Rotation::Quaternion(
                          m_odometry_state.pose.pose.orientation.x,
                          m_odometry_state.pose.pose.orientation.y,
                          m_odometry_state.pose.pose.orientation.z,
                          m_odometry_state.pose.pose.orientation.w);

      // Instead of transforming for the inverse of this rotation,
      // we should find the right transform. (this is legacy code that
      // transforms vrep coords to rviz coords)
      KDL::Rotation rotation = KDL::Rotation::RPY(0,-M_PI/2,M_PI).Inverse();
      
      orientation = orientation * rotation;
      orientation.GetRPY(euler[0],euler[1],euler[2]); 

      euler_s[0] = euler[0];
      euler_s[1] = euler[1];
      euler_s[2] = euler[2];

      simxSetObjectOrientation(clientID,all_robot_handle,-1,euler_s,simx_opmode_oneshot);

      simxSetJointPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
      return;
    }

      Hilas::IRobotBaseService::update();
  }