void Object::setGlobalPosition(std::tuple<float, float, float>& position) { int N = 3; simxFloat positionVec[N]; positionVec[0] = std::get<0>(position); positionVec[1] = std::get<1>(position); positionVec[2] = std::get<2>(position); try { ReturnCodesExceptions::handleReturnCodeNoResponse( simxSetObjectPosition(m_ClientId, m_Handle, -1, positionVec, simx_opmode_oneshot)); } catch (ReturnCodesExceptions *ex) { throw ex; } }
void RobotVREP::setObjectPosition(Object * object, vector < double > position, int relativeTo, simxInt operationMode) { int uniqueId = object->getUniqueObjectId(); int vrepHandlerVectorPosition = objectIdToVrepHandler_map.at(uniqueId); int * handler = VrepHandlerVector.at(vrepHandlerVectorPosition); float * pos = new float[3]; pos[0] = (float)position.at(0); pos[1] = (float)position.at(1); pos[2] = (float)position.at(2); int error = simxSetObjectPosition(clientID, *handler, relativeTo, pos, operationMode); if(error != 0) vrep_error << "simxSetObjectPosition: " << *handler << " : " << error << endl; delete[] pos; }
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(); }