├── src └── robosim │ ├── sslconfig.cpp │ ├── vssconfig.cpp │ ├── __init__.py │ ├── utils.h │ ├── utils.cpp │ ├── physics │ ├── pfixedbox.h │ ├── pball.h │ ├── pfixedbox.cpp │ ├── pbox.h │ ├── pcylinder.h │ ├── pground.h │ ├── pground.cpp │ ├── pball.cpp │ ├── pbox.cpp │ ├── pcylinder.cpp │ ├── pobject.h │ ├── pworld.h │ ├── pobject.cpp │ └── pworld.cpp │ ├── sslworld.h │ ├── vssworld.h │ ├── vssrobot.h │ ├── sslrobot.h │ ├── robosim_py.cpp │ ├── vssconfig.h │ ├── sslconfig.h │ ├── vssrobot.cpp │ ├── sslrobot.cpp │ ├── vssworld.cpp │ └── sslworld.cpp ├── .gitignore ├── .github ├── action │ ├── action.yml │ ├── entrypoint.sh │ └── Dockerfile └── workflows │ ├── release-pypi.yml │ └── release-testpypi.yml ├── pyproject.toml ├── setup.py ├── CMakeLists.txt ├── FindODE.cmake └── README.md /src/robosim/sslconfig.cpp: -------------------------------------------------------------------------------- 1 | #include "sslconfig.h" -------------------------------------------------------------------------------- /src/robosim/vssconfig.cpp: -------------------------------------------------------------------------------- 1 | #include "vssconfig.h" -------------------------------------------------------------------------------- /src/robosim/__init__.py: -------------------------------------------------------------------------------- 1 | from ._robosim import VSS, SSL -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | .idea/ 3 | 4 | cmake-build-debug/ 5 | 6 | _skbuild/ 7 | 8 | src/robosim_pybind11.egg-info/ 9 | -------------------------------------------------------------------------------- /.github/action/action.yml: -------------------------------------------------------------------------------- 1 | # action.yml 2 | name: 'World' 3 | description: 'Build docker and run entrypoint script' 4 | runs: 5 | using: 'docker' 6 | image: 'Dockerfile' -------------------------------------------------------------------------------- /src/robosim/utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define PI 3.14159265358979323846 4 | 5 | dReal smallestAngleDiff(dReal angle, dReal last_angle); 6 | dReal fric(dReal f); -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools", "wheel", "scikit-build", "cmake", "ninja", "setuptools_scm>=6.2"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [tool.setuptools_scm] 6 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | try: 4 | from skbuild import setup 5 | except ImportError: 6 | print('Please update pip, you need pip 10 or greater,\n' 7 | ' or you need to install the PEP 518 requirements in pyproject.toml yourself', file=sys.stderr) 8 | raise 9 | 10 | setup( 11 | name="rc-robosim", 12 | url="https://github.com/robocin/rSim", 13 | description="SSL and VSS robot soccer simulator", 14 | author='Felipe Martins', 15 | author_email="fbm2@cin.ufpe.br", 16 | packages=['robosim'], 17 | package_dir={'': 'src'}, 18 | cmake_install_dir='src/robosim' 19 | ) 20 | -------------------------------------------------------------------------------- /src/robosim/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | dReal smallestAngleDiff(dReal angle, dReal last_angle) 4 | { 5 | // TODO Assert as false if the robot is capable of rotating more than 180 6 | // degrees in one time step 7 | dReal diff = (angle - last_angle); 8 | 9 | if ((diff + 180.0) > 360.0) 10 | { 11 | diff = diff - 360.0; 12 | } else if ((diff - 180.0) < -360.0) 13 | { 14 | diff = diff + 360.0; 15 | } 16 | 17 | return diff; 18 | } 19 | 20 | dReal fric(dReal f) 21 | { 22 | if (f + 1 < 0.001) 23 | return dInfinity; 24 | return f; 25 | } 26 | -------------------------------------------------------------------------------- /.github/action/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh -l 2 | 3 | echo "Starting entrypoint.sh" 4 | 5 | echo "Build wheels for different python versions" 6 | for PYBIN in /opt/python/*/bin; do #3.7,3.8,3.9 7 | echo "builind dist for ${PYBIN}" 8 | "${PYBIN}/pip" install pip --upgrade #precisa para buildar 9 | "${PYBIN}/pip" install scikit-build #precisa para buildar 10 | "${PYBIN}/python" -m build --wheel --outdir dist/ . 11 | done 12 | 13 | echo "auditwheels" 14 | for WHEEL in dist/*; do #3.7,3.8,3.9 15 | auditwheel repair $WHEEL 16 | done 17 | 18 | rm -rf dist/ 19 | mv -v wheelhouse/ dist/ 20 | 21 | echo "setup sdist" 22 | /opt/python/pp37-pypy37_pp73/bin/python -m build --sdist --outdir dist/ -------------------------------------------------------------------------------- /.github/action/Dockerfile: -------------------------------------------------------------------------------- 1 | # Container image that runs your code 2 | FROM quay.io/pypa/manylinux2010_x86_64 3 | 4 | # Install ODE tag 0.16.2 with libccd 5 | RUN git clone --depth 1 --branch 0.16.2 https://bitbucket.org/odedevs/ode.git \ 6 | && export LDFLAGS=-lrt \ 7 | && mkdir ode-build \ 8 | && cd ode-build \ 9 | && cmake ../ode -DCMAKE_BUILD_TYPE=Release -DODE_WITH_DEMOS=OFF -DODE_WITH_LIBCCD=ON \ 10 | && make \ 11 | && make install 12 | 13 | # Copies your code file from your action repository to the filesystem path `/` of the container 14 | COPY entrypoint.sh /entrypoint.sh 15 | 16 | # Code file to execute when the docker container starts up (`entrypoint.sh`) 17 | ENTRYPOINT ["/entrypoint.sh"] -------------------------------------------------------------------------------- /src/robosim/physics/pfixedbox.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef PFIXEDBOX_H 20 | #define PFIXEDBOX_H 21 | 22 | #include "pobject.h" 23 | 24 | class PFixedBox : public PObject 25 | { 26 | private: 27 | dReal m_w, m_h, m_l; 28 | 29 | public: 30 | PFixedBox(dReal x, dReal y, dReal z, dReal w, dReal h, dReal l); 31 | ~PFixedBox() override; 32 | void init() override; 33 | }; 34 | 35 | #endif // PFIXEDBOX_H 36 | -------------------------------------------------------------------------------- /src/robosim/physics/pball.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef PBALL_H 20 | #define PBALL_H 21 | 22 | #include "pobject.h" 23 | 24 | class PBall : public PObject 25 | { 26 | private: 27 | dReal m_radius; 28 | 29 | public: 30 | PBall(dReal x, dReal y, dReal z, dReal radius, dReal mass); 31 | ~PBall() override; 32 | void setMass(dReal mass) override; 33 | void init() override; 34 | }; 35 | 36 | #endif // PBALL_H 37 | -------------------------------------------------------------------------------- /src/robosim/physics/pfixedbox.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "pfixedbox.h" 20 | 21 | PFixedBox::PFixedBox(dReal x, dReal y, dReal z, dReal w, dReal h, dReal l) 22 | : PObject(x, y, z, 0) 23 | { 24 | m_w = w; 25 | m_h = h; 26 | m_l = l; 27 | } 28 | 29 | PFixedBox::~PFixedBox() = default; 30 | 31 | void PFixedBox::init() 32 | { 33 | geom = dCreateBox(space, m_w, m_h, m_l); 34 | initPosGeom(); 35 | } 36 | -------------------------------------------------------------------------------- /src/robosim/physics/pbox.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef PBOX_H 20 | #define PBOX_H 21 | 22 | #include "pobject.h" 23 | 24 | class PBox : public PObject 25 | { 26 | private: 27 | dReal m_w, m_h, m_l; 28 | 29 | public: 30 | PBox(dReal x, dReal y, dReal z, dReal w, dReal h, dReal l, dReal mass); 31 | ~PBox() override; 32 | void setMass(dReal mass) override; 33 | void init() override; 34 | }; 35 | 36 | #endif // PBOX_H 37 | -------------------------------------------------------------------------------- /src/robosim/physics/pcylinder.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef PCYLINDER_H 20 | #define PCYLINDER_H 21 | 22 | #include "pobject.h" 23 | 24 | class PCylinder : public PObject 25 | { 26 | private: 27 | dReal m_radius, m_length; 28 | 29 | public: 30 | PCylinder(dReal x, dReal y, dReal z, dReal radius, dReal length, dReal mass); 31 | ~PCylinder() override; 32 | void setMass(dReal mass) override; 33 | void init() override; 34 | }; 35 | 36 | #endif // PCYLINDER_H 37 | -------------------------------------------------------------------------------- /src/robosim/physics/pground.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef PGROUND_H 20 | #define PGROUND_H 21 | 22 | #include "pobject.h" 23 | 24 | class PGround : public PObject 25 | { 26 | private: 27 | dReal rad, len, wid, pdep, pwid, ppoint, lwidth; 28 | 29 | public: 30 | PGround(dReal field_radius, dReal field_length, dReal field_width, dReal field_penalty_rad, dReal field_penalty_line_length, dReal field_penalty_point, dReal field_line_width); 31 | ~PGround() override; 32 | void init() override; 33 | }; 34 | 35 | #endif // PGROUND_H 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | project(robosim) 4 | set(CMAKE_CXX_STANDARD 11) 5 | 6 | # Define CMAKE_INSTALL_xxx: LIBDIR, INCLUDEDIR 7 | include(GNUInstallDirs) 8 | include(FindPackageHandleStandardArgs) 9 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}) 10 | # Fetch pybind11 11 | include(FetchContent) 12 | 13 | FetchContent_Declare( 14 | pybind11 15 | URL https://github.com/pybind/pybind11/archive/refs/tags/v2.6.2.tar.gz 16 | URL_HASH SHA256=8ff2fff22df038f5cd02cea8af56622bc67f5b64534f1b83b9f133b8366acff2 17 | ) 18 | FetchContent_MakeAvailable(pybind11) 19 | 20 | set(python_module_name _robosim) 21 | 22 | file(GLOB SRC_FILES src/robosim/*.cpp) 23 | file(GLOB HEADER_FILES src/robosim/*.h) 24 | file(GLOB P_SRC_FILES src/robosim/physics/*.cpp) 25 | file(GLOB P_HEADER_FILES src/robosim/physics/*.h) 26 | 27 | pybind11_add_module(${python_module_name} MODULE 28 | ${SRC_FILES} 29 | ${HEADER_FILES} 30 | ${P_SRC_FILES} 31 | ${P_HEADER_FILES} 32 | ) 33 | 34 | find_package(ODE REQUIRED) 35 | include_directories(${ODE_INCLUDE_DIRS}) 36 | 37 | 38 | #target_link_libraries(${python_module_name} PUBLIC MyLib) 39 | target_link_libraries(${python_module_name} PUBLIC ${ODE_LIBRARIES}) 40 | 41 | install(TARGETS ${python_module_name} DESTINATION .) 42 | 43 | # Quiet a warning, since this project is only valid with SKBUILD 44 | set(ignoreMe "${SKBUILD}") 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /FindODE.cmake: -------------------------------------------------------------------------------- 1 | find_path( 2 | ODE_INCLUDE_DIRS 3 | NAMES 4 | ode/ode.h 5 | HINTS 6 | $ENV{HOME}/include 7 | /usr/local/include 8 | /usr/include 9 | $ENV{ProgramFiles}/ode/include 10 | ) 11 | 12 | find_library( 13 | ODE_LIBRARY_DEBUG 14 | NAMES 15 | oded debugdll/ode 16 | HINTS 17 | $ENV{HOME}/lib 18 | /usr/local/lib 19 | /usr/lib 20 | $ENV{ProgramFiles}/ode/lib 21 | ) 22 | 23 | find_library( 24 | ODE_LIBRARY_RELEASE 25 | NAMES 26 | ode releasedll/ode 27 | HINTS 28 | $ENV{HOME}/lib 29 | /usr/local/lib 30 | /usr/lib 31 | $ENV{ProgramFiles}/ode/lib 32 | ) 33 | 34 | if(ODE_LIBRARY_DEBUG AND NOT ODE_LIBRARY_RELEASE) 35 | set(ODE_LIBRARIES ${ODE_LIBRARY_DEBUG}) 36 | endif() 37 | 38 | if(ODE_LIBRARY_RELEASE AND NOT ODE_LIBRARY_DEBUG) 39 | set(ODE_LIBRARIES ${ODE_LIBRARY_RELEASE}) 40 | endif() 41 | 42 | if(ODE_LIBRARY_DEBUG AND ODE_LIBRARY_RELEASE) 43 | set(ODE_LIBRARIES debug ${ODE_LIBRARY_DEBUG} optimized ${ODE_LIBRARY_RELEASE}) 44 | endif() 45 | 46 | find_package_handle_standard_args( 47 | ODE 48 | DEFAULT_MSG 49 | ODE_INCLUDE_DIRS 50 | ODE_LIBRARIES 51 | ) 52 | 53 | mark_as_advanced( 54 | ODE_INCLUDE_DIRS 55 | ODE_LIBRARIES 56 | ODE_LIBRARY_DEBUG 57 | ODE_LIBRARY_RELEASE 58 | ) 59 | -------------------------------------------------------------------------------- /src/robosim/physics/pground.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "pground.h" 20 | 21 | PGround::PGround(dReal field_radius, dReal field_length, dReal field_width, dReal field_penalty_rad, dReal field_penalty_line_length, dReal field_penalty_point, dReal field_line_width) 22 | : PObject(0, 0, 0, 0) 23 | { 24 | rad = field_radius; 25 | len = field_length; 26 | wid = field_width; 27 | pdep = field_penalty_rad; 28 | pwid = field_penalty_line_length; 29 | ppoint = field_penalty_point; 30 | lwidth = field_line_width; 31 | } 32 | 33 | void PGround::init() 34 | { 35 | geom = dCreatePlane(space, 0, 0, 1, 0); 36 | } 37 | 38 | PGround::~PGround() = default; 39 | -------------------------------------------------------------------------------- /src/robosim/physics/pball.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "pball.h" 20 | 21 | PBall::PBall(dReal x, dReal y, dReal z, dReal radius, dReal mass) 22 | : PObject(x, y, z, mass) 23 | { 24 | m_radius = radius; 25 | } 26 | 27 | PBall::~PBall() = default; 28 | 29 | void PBall::init() 30 | { 31 | body = dBodyCreate(world); 32 | initPosBody(); 33 | setMass(m_mass); 34 | geom = dCreateSphere(nullptr, m_radius); 35 | dGeomSetBody(geom, body); 36 | dSpaceAdd(space, geom); 37 | } 38 | 39 | void PBall::setMass(dReal mass) 40 | { 41 | 42 | m_mass = mass; 43 | dMass m; 44 | dMassSetSphereTotal(&m, m_mass, m_radius); 45 | dBodySetMass(body, &m); 46 | } 47 | -------------------------------------------------------------------------------- /src/robosim/physics/pbox.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "pbox.h" 20 | 21 | PBox::PBox(dReal x, dReal y, dReal z, dReal w, dReal h, dReal l, dReal mass) 22 | : PObject(x, y, z, mass) 23 | { 24 | 25 | m_w = w; 26 | m_h = h; 27 | m_l = l; 28 | } 29 | 30 | PBox::~PBox() = default; 31 | 32 | void PBox::init() 33 | { 34 | body = dBodyCreate(world); 35 | initPosBody(); 36 | setMass(m_mass); 37 | geom = dCreateBox(nullptr, m_w, m_h, m_l); 38 | dGeomSetBody(geom, body); 39 | dSpaceAdd(space, geom); 40 | } 41 | 42 | void PBox::setMass(dReal mass) 43 | { 44 | m_mass = mass; 45 | dMass m; 46 | dMassSetBoxTotal(&m, m_mass, m_w, m_h, m_l); 47 | dBodySetMass(body, &m); 48 | } 49 | -------------------------------------------------------------------------------- /src/robosim/physics/pcylinder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "pcylinder.h" 20 | 21 | PCylinder::PCylinder(dReal x, dReal y, dReal z, dReal radius, dReal length, dReal mass) 22 | : PObject(x, y, z, mass) 23 | { 24 | m_radius = radius; 25 | m_length = length; 26 | } 27 | 28 | PCylinder::~PCylinder() = default; 29 | 30 | void PCylinder::setMass(dReal mass) 31 | { 32 | m_mass = mass; 33 | dMass m; 34 | dMassSetCylinderTotal(&m, m_mass, 1, m_radius, m_length); 35 | dBodySetMass(body, &m); 36 | } 37 | 38 | void PCylinder::init() 39 | { 40 | body = dBodyCreate(world); 41 | initPosBody(); 42 | setMass(m_mass); 43 | geom = dCreateCylinder(nullptr, m_radius, m_length); 44 | dGeomSetBody(geom, body); 45 | dSpaceAdd(space, geom); 46 | } 47 | -------------------------------------------------------------------------------- /.github/workflows/release-pypi.yml: -------------------------------------------------------------------------------- 1 | name: Build and publish tag on PyPI 2 | 3 | on: 4 | workflow_dispatch: 5 | inputs: 6 | tag: 7 | description: 'Tag to publish' 8 | required: true 9 | 10 | jobs: 11 | publish_ubuntu: 12 | name: Publishing package on linux manywheels 13 | runs-on: ubuntu-latest 14 | 15 | steps: 16 | - name: Checkout 17 | uses: actions/checkout@v2 18 | with: 19 | fetch-depth: 0 20 | ref: ${{ github.event.inputs.tag }} 21 | 22 | - name: manylinux2010_x86_64 Docker build action 23 | uses: ./.github/action/ # Uses an action in the root directory 24 | 25 | - name: Publish distribution 📦 to PyPI 26 | uses: pypa/gh-action-pypi-publish@master 27 | with: 28 | password: ${{ secrets.PYPI_API_TOKEN }} 29 | 30 | # # TODO Build and publish on Windows and MacOS 31 | # build_windows: 32 | # runs-on: windows-latest 33 | # name: Building package on windows 34 | # steps: 35 | # - name: Checkout 36 | # uses: actions/checkout@v2 37 | # with: 38 | # ref: ${{ github.event.inputs.tag }} 39 | # - name: Clone ODE 0.16.2 40 | # run: git clone --depth 1 --branch 0.16.2 https://bitbucket.org/odedevs/ode.git 41 | # - name: Run cmake on ode 42 | # run: | 43 | # mkdir ode-build 44 | # cd ode-build 45 | # cmake ../ode -DCMAKE_BUILD_TYPE=Release -DODE_WITH_DEMOS=OFF -DODE_WITH_LIBCCD=ON 46 | # - name: Install ODE 47 | # working-directory: ${{ github.workspace }}/ode-build/ 48 | # run: cmake --build . --target INSTALL --config Release 49 | -------------------------------------------------------------------------------- /.github/workflows/release-testpypi.yml: -------------------------------------------------------------------------------- 1 | name: Build and publish tag on Test PyPI 2 | 3 | on: 4 | workflow_dispatch: 5 | inputs: 6 | tag: 7 | description: 'Tag to publish' 8 | required: true 9 | 10 | jobs: 11 | publish_ubuntu: 12 | name: Publishing package on linux manywheels 13 | runs-on: ubuntu-latest 14 | 15 | steps: 16 | - name: Checkout 17 | uses: actions/checkout@v2 18 | with: 19 | fetch-depth: 0 20 | ref: ${{ github.event.inputs.tag }} 21 | 22 | - name: manylinux2010_x86_64 Docker build action 23 | uses: ./.github/action/ # Uses an action in the root directory 24 | 25 | - name: Publish distribution 📦 to Test PyPI 26 | uses: pypa/gh-action-pypi-publish@master 27 | with: 28 | password: ${{ secrets.TEST_PYPI_API_TOKEN }} 29 | repository_url: https://test.pypi.org/legacy/ 30 | 31 | # # TODO Build and publish on Windows and MacOS 32 | # build_windows: 33 | # runs-on: windows-latest 34 | # name: Building package on windows 35 | # steps: 36 | # - name: Checkout 37 | # uses: actions/checkout@v2 38 | # with: 39 | # ref: ${{ github.event.inputs.tag }} 40 | # - name: Clone ODE 0.16.2 41 | # run: git clone --depth 1 --branch 0.16.2 https://bitbucket.org/odedevs/ode.git 42 | # - name: Run cmake on ode 43 | # run: | 44 | # mkdir ode-build 45 | # cd ode-build 46 | # cmake ../ode -DCMAKE_BUILD_TYPE=Release -DODE_WITH_DEMOS=OFF -DODE_WITH_LIBCCD=ON 47 | # - name: Install ODE 48 | # working-directory: ${{ github.workspace }}/ode-build/ 49 | # run: cmake --build . --target INSTALL --config Release 50 | -------------------------------------------------------------------------------- /src/robosim/physics/pobject.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef POBJECT_H 20 | #define POBJECT_H 21 | #include 22 | 23 | class PObject 24 | { 25 | private: 26 | bool isQSet; 27 | 28 | protected: 29 | dReal m_x, m_y, m_z; 30 | dReal m_mass; 31 | dMatrix3 local_Rot{}; 32 | dVector3 local_Pos{}; 33 | dQuaternion q{}; 34 | void initPosBody(); 35 | void initPosGeom(); 36 | 37 | public: 38 | PObject(dReal x, dReal y, dReal z, dReal mass); 39 | virtual ~PObject(); 40 | void setRotation(dReal x_axis, dReal y_axis, dReal z_axis, dReal ang); //Must be called before init() 41 | void setBodyPosition(dReal x, dReal y, dReal z, bool local = false); 42 | void setBodyRotation(dReal x_axis, dReal y_axis, dReal z_axis, dReal ang, bool local = false); 43 | void getBodyPosition(dReal &x, dReal &y, dReal &z, bool local = false); 44 | void getBodyDirection(dReal &x, dReal &y, dReal &z); 45 | void getBodyDirection(dReal &x, dReal &y, dReal &z, dReal &k); 46 | void getBodyRotation(dMatrix3 r, bool local = false); 47 | virtual void setMass(dReal mass); 48 | virtual void init() = 0; 49 | 50 | dBodyID body; 51 | dGeomID geom; 52 | dWorldID world; 53 | dSpaceID space; 54 | int tag; 55 | int id{}; 56 | }; 57 | 58 | #endif // POBJECT_H 59 | -------------------------------------------------------------------------------- /src/robosim/sslworld.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef SSLWORLD_H 20 | #define SSLWORLD_H 21 | 22 | 23 | #include "physics/pworld.h" 24 | #include "physics/pball.h" 25 | #include "physics/pground.h" 26 | #include "physics/pfixedbox.h" 27 | 28 | #include "sslrobot.h" 29 | #include "utils.h" 30 | #include 31 | 32 | #define SSL_WALL_COUNT 10 33 | #define SSL_MAX_ROBOT_COUNT 22 //don't change 34 | 35 | class SSLWorld 36 | { 37 | private: 38 | double timeStep; 39 | int stateSize; 40 | std::vector state; 41 | 42 | public: 43 | bool fullSpeed = false; 44 | PWorld *physics; 45 | PBall *ball; 46 | PGround *ground; 47 | PFixedBox *walls[SSL_WALL_COUNT]{}; 48 | SSLRobot *robots[SSL_MAX_ROBOT_COUNT * 2]{}; 49 | SSLConfig::Field field = SSLConfig::Field(); 50 | 51 | SSLWorld(int fieldType, int nRobotsBlue, int nRobotsYellow, double timeStep, 52 | std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos); 53 | ~SSLWorld(); 54 | void step(std::vector> actions); 55 | void replace(std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos); 56 | void initWalls(); 57 | int getNumRobotsBlue() { return this->field.getRobotsBlueCount(); } 58 | int getNumRobotsYellow() { return this->field.getRobotsYellowCount(); } 59 | const std::unordered_map getFieldParams(); 60 | const std::vector &getState(); 61 | void setActions(std::vector> actions); 62 | }; 63 | 64 | #endif // SSLWorld_H 65 | -------------------------------------------------------------------------------- /src/robosim/vssworld.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef VSSWORLD_H 20 | #define VSSWORLD_H 21 | 22 | 23 | #include "physics/pworld.h" 24 | #include "physics/pball.h" 25 | #include "physics/pground.h" 26 | #include "physics/pfixedbox.h" 27 | 28 | #include "vssrobot.h" 29 | #include "utils.h" 30 | #include 31 | 32 | #define VSS_WALL_COUNT 16 33 | #define VSS_MAX_ROBOT_COUNT 12 //don't change 34 | 35 | class VSSWorld 36 | { 37 | private: 38 | double timeStep; 39 | int stateSize; 40 | std::vector state; 41 | 42 | public: 43 | VSSConfig::Field field = VSSConfig::Field(); 44 | bool fullSpeed = false; 45 | PWorld *physics; 46 | PBall *ball; 47 | PGround *ground; 48 | PFixedBox *walls[VSS_WALL_COUNT]{}; 49 | VSSRobot *robots[VSS_MAX_ROBOT_COUNT * 2]{}; 50 | 51 | VSSWorld(int fieldType, int nRobotsBlue, int nRobotsYellow, double timeStep, 52 | std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos); 53 | ~VSSWorld(); 54 | void step(std::vector> actions); 55 | void replace(std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos); 56 | void initWalls(); 57 | int getNumRobotsBlue() { return this->field.getRobotsBlueCount(); } 58 | int getNumRobotsYellow() { return this->field.getRobotsYellowCount(); } 59 | const std::unordered_map getFieldParams(); 60 | const std::vector &getState(); 61 | int robotIndex(unsigned int robot, int team); 62 | void setActions(std::vector>); 63 | }; 64 | 65 | #endif // World_H 66 | -------------------------------------------------------------------------------- /src/robosim/physics/pworld.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef PWORLD_H 20 | #define PWORLD_H 21 | 22 | #include 23 | #include "pobject.h" 24 | 25 | class PSurface; 26 | class PWorld 27 | { 28 | private: 29 | dJointGroupID contactgroup; 30 | std::vector objects; 31 | std::vector surfaces; 32 | dReal delta_time; 33 | int **sur_matrix; 34 | int objects_count; 35 | 36 | public: 37 | PWorld(dReal dt, dReal gravity, int robot_count); 38 | ~PWorld(); 39 | void setGravity(dReal gravity); 40 | int addObject(PObject* o); 41 | int addBallObject(PObject* o); 42 | int addGroundObject(PObject* o); 43 | int addWallObject(PObject* o); 44 | int addWheelObject(PObject* o); 45 | int addChassisObject(PObject* o); 46 | int addKickerObject(PObject* o); 47 | void initAllObjects(); 48 | PSurface *createSurface(PObject *o1, PObject *o2); 49 | PSurface* createOneWaySurface(PObject* o1,PObject* o2); 50 | PSurface *findSurface(PObject *o1, PObject *o2); 51 | void step(dReal dt = -1, bool sync = false); 52 | void handleCollisions(dGeomID o1, dGeomID o2); 53 | dWorldID world; 54 | dSpaceID space, spaceChassis, spaceKicker, spaceWall, spaceWheel; 55 | PObject *ball, *ground; 56 | int robot_count; 57 | }; 58 | 59 | typedef bool PSurfaceCallback(dGeomID o1, dGeomID o2, PSurface *s, int robot_count); 60 | class PSurface 61 | { 62 | public: 63 | PSurface(); 64 | dSurfaceParameters surface{}; 65 | bool isIt(dGeomID i1, dGeomID i2); 66 | dGeomID id1{}, id2{}; 67 | bool usefdir1; //if true use fdir1 instead of ODE value 68 | dVector3 fdir1{}; //fdir1 is a normalized vector tangent to friction force vector 69 | dVector3 contactPos{}, contactNormal{}; 70 | PSurfaceCallback *callback; 71 | }; 72 | #endif // PWORLD_H 73 | -------------------------------------------------------------------------------- /src/robosim/vssrobot.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef VSSROBOT_H 20 | #define VSSROBOT_H 21 | 22 | #include "physics/pworld.h" 23 | #include "physics/pcylinder.h" 24 | #include "physics/pbox.h" 25 | #include "physics/pball.h" 26 | #include "vssconfig.h" 27 | 28 | #define ROBOT_GRAY 0.4 29 | 30 | class VSSRobot 31 | { 32 | PWorld *physics; 33 | PBall *m_ball; 34 | dReal m_x, m_y, m_z; 35 | dReal m_r, m_g, m_b; 36 | dReal m_dir; 37 | int m_rob_id; 38 | bool last_state{}; 39 | 40 | public: 41 | dSpaceID space; 42 | PObject *chassis; 43 | PBox *boxes[3]{}; 44 | bool on; 45 | //these values are not controled by this class 46 | bool selected{}; 47 | dReal select_x{}, select_y{}, select_z{}; 48 | class Wheel 49 | { 50 | public: 51 | int id; 52 | Wheel(VSSRobot *robot, int _id, dReal ang, dReal ang2); 53 | void step(); 54 | ~Wheel(); 55 | dJointID joint; 56 | dJointID motor; 57 | PCylinder *cyl; 58 | dReal desiredAngularSpeed, maxAngularSpeed; // rad/s 59 | VSSRobot *rob; 60 | } * wheels[2]{}; 61 | 62 | class RBall 63 | { 64 | public: 65 | int id; 66 | RBall(VSSRobot *robot, int _id, dReal ang, dReal ang2); 67 | void step(); 68 | ~RBall(); 69 | dJointID joint; 70 | PBall *pBall; 71 | VSSRobot *rob; 72 | } * balls[4]{}; 73 | 74 | VSSRobot(PWorld *world, PBall *ball, dReal x, dReal y, dReal z, 75 | int rob_id, int dir, bool turn_on); 76 | ~VSSRobot(); 77 | void step(); 78 | void setWheelDesiredAngularSpeed(int i, dReal s); //i = 0,1,2,3 79 | void setSpeed(dReal vx, dReal vy, dReal vw); 80 | dReal getSpeed(int i); 81 | void incSpeed(int i, dReal v); 82 | void resetSpeeds(); 83 | void resetRobot(); 84 | void getXY(dReal &x, dReal &y); 85 | dReal getDir(dReal &k); 86 | void setXY(dReal x, dReal y); 87 | void setDir(dReal ang); 88 | int getID(); 89 | PBall *getBall(); 90 | PWorld *getWorld(); 91 | }; 92 | 93 | #define VSS_ROBOT_START_Z() (VSSConfig::Robot().getHeight() * 0.5 + VSSConfig::Robot().getBottomHeight()) 94 | 95 | #endif // ROBOT_H 96 | -------------------------------------------------------------------------------- /src/robosim/sslrobot.h: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #ifndef SSLROBOT_H 20 | #define SSLROBOT_H 21 | 22 | #include "physics/pworld.h" 23 | #include "physics/pcylinder.h" 24 | #include "physics/pbox.h" 25 | #include "physics/pball.h" 26 | #include "sslconfig.h" 27 | 28 | enum KickStatus { 29 | NO_KICK = 0, 30 | FLAT_KICK = 1, 31 | CHIP_KICK = 2, 32 | }; 33 | 34 | class SSLRobot 35 | { 36 | PWorld *physics; 37 | PBall *ball; 38 | int rob_id; 39 | 40 | public: 41 | dReal _x, _y, _z; 42 | dReal _dir; 43 | dSpaceID space; 44 | PObject *chassis; 45 | 46 | class Wheel 47 | { 48 | public: 49 | int id; 50 | Wheel(SSLRobot *robot, int _id, dReal ang, dReal ang2); 51 | ~Wheel(); 52 | void step(); 53 | dJointID joint; 54 | dJointID motor; 55 | PCylinder *cyl; 56 | dReal desiredAngularSpeed, maxAngularSpeed; // rad/s 57 | SSLRobot *rob; 58 | } * wheels[4]{}; 59 | class Kicker 60 | { 61 | private: 62 | KickStatus kickerState; 63 | bool dribblerOn; 64 | int kickerCounter; 65 | bool holdingBall; 66 | public: 67 | Kicker(SSLRobot* robot); 68 | ~Kicker(); 69 | void step(); 70 | void kick(dReal kickSpeedX, dReal kickSpeedZ); 71 | void setDribbler(bool dribbler); 72 | bool getDribbler(); 73 | void toggleDribbler(); 74 | bool isTouchingBall(); 75 | KickStatus getKickerStatus(); 76 | void holdBall(); 77 | void unholdBall(); 78 | dJointID joint; 79 | dJointID robot_to_ball; 80 | PBox* box; 81 | SSLRobot* rob; 82 | } *kicker; 83 | 84 | SSLRobot(PWorld *world, PBall *ball, dReal x, dReal y, dReal z, 85 | int robot_id, dReal dir); 86 | ~SSLRobot(); 87 | void step(); 88 | void setDesiredSpeedLocal(dReal vx, dReal vy, dReal vw); 89 | void setWheelDesiredAngularSpeed(int i, dReal s); //i = 0,1,2,3 90 | void setSpeed(dReal vx, dReal vy, dReal vw); 91 | void incSpeed(int i, dReal v); 92 | void resetSpeeds(); 93 | void resetRobot(); 94 | void getXY(dReal &x, dReal &y); 95 | dReal getDir(dReal &k); 96 | void setXY(dReal x, dReal y); 97 | void setDir(dReal ang); 98 | int getID(); 99 | PBall *getBall(); 100 | PWorld *getWorld(); 101 | }; 102 | 103 | #define SSL_ROBOT_START_Z() (SSLConfig::Robot().getHeight() * 0.5 + SSLConfig::Robot().getBottomHeight()) 104 | 105 | #endif // SSLROBOT_H 106 | -------------------------------------------------------------------------------- /src/robosim/robosim_py.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "vssworld.h" 6 | #include "sslworld.h" 7 | 8 | namespace py = pybind11; 9 | using vd = std::vector; 10 | using vvd = std::vector>; 11 | 12 | struct VSS { 13 | VSS(int fieldType, int nRobotsBlue, int nRobotsYellow, int timeStep_ms, 14 | const vd &ballPos, const vvd &blueRobotsPos, const vvd &yellowRobotsPos) : m_fieldType(fieldType), 15 | m_nRobotsBlue(nRobotsBlue), 16 | m_nRobotsYellow(nRobotsYellow), 17 | m_timeStep_ms(timeStep_ms) { 18 | m_world = new VSSWorld(m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms / 1000.0, 19 | ballPos, blueRobotsPos, yellowRobotsPos); 20 | } 21 | 22 | ~VSS() { delete m_world; } 23 | 24 | void step(vvd actions) const { m_world->step(std::move(actions)); } 25 | 26 | vd getState() const { return m_world->getState(); } 27 | 28 | void reset(const vd &ballPos, const vvd &blueRobotsPos, const vvd &yellowRobotsPos) { 29 | delete m_world; 30 | m_world = new VSSWorld(m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms / 1000.0, 31 | ballPos, blueRobotsPos, yellowRobotsPos); 32 | } 33 | 34 | std::unordered_map getFieldParams() const { return m_world->getFieldParams(); } 35 | 36 | VSSWorld *m_world; 37 | int m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms; 38 | }; 39 | 40 | struct SSL { 41 | SSL(int fieldType, int nRobotsBlue, int nRobotsYellow, int timeStep_ms, 42 | const vd &ballPos, const vvd &blueRobotsPos, const vvd &yellowRobotsPos) : m_fieldType(fieldType), 43 | m_nRobotsBlue(nRobotsBlue), 44 | m_nRobotsYellow(nRobotsYellow), 45 | m_timeStep_ms(timeStep_ms) { 46 | m_world = new SSLWorld(m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms / 1000.0, 47 | ballPos, blueRobotsPos, yellowRobotsPos); 48 | } 49 | 50 | ~SSL() { delete m_world; } 51 | 52 | void step(vvd actions) const { m_world->step(std::move(actions)); } 53 | 54 | vd getState() const { return m_world->getState(); } 55 | 56 | void reset(const vd &ballPos, const vvd &blueRobotsPos, const vvd &yellowRobotsPos) { 57 | delete m_world; 58 | m_world = new SSLWorld(m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms / 1000.0, 59 | ballPos, blueRobotsPos, yellowRobotsPos); 60 | } 61 | 62 | std::unordered_map getFieldParams() const { return m_world->getFieldParams(); } 63 | 64 | SSLWorld *m_world; 65 | int m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms; 66 | }; 67 | 68 | 69 | PYBIND11_MODULE(_robosim, m) { 70 | py::class_(m, "VSS") 71 | .def(py::init()) 72 | .def("step", &VSS::step) 73 | .def("get_state", &VSS::getState) 74 | .def("reset", &VSS::reset) 75 | .def("get_field_params", &VSS::getFieldParams); 76 | 77 | py::class_(m, "SSL") 78 | .def(py::init()) 79 | .def("step", &SSL::step) 80 | .def("get_state", &SSL::getState) 81 | .def("reset", &SSL::reset) 82 | .def("get_field_params", &SSL::getFieldParams); 83 | } 84 | 85 | 86 | -------------------------------------------------------------------------------- /src/robosim/physics/pobject.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "pobject.h" 20 | 21 | PObject::PObject(dReal x, dReal y, dReal z, dReal mass) 22 | { 23 | geom = nullptr; 24 | body = nullptr; 25 | world = nullptr; 26 | space = nullptr; 27 | m_x = x; 28 | m_y = y; 29 | m_z = z; 30 | m_mass = mass; 31 | isQSet = false; 32 | tag = 0; 33 | } 34 | 35 | PObject::~PObject() 36 | { 37 | if (geom != nullptr) 38 | dGeomDestroy(geom); 39 | if (body != nullptr) 40 | dBodyDestroy(body); 41 | } 42 | 43 | void PObject::setRotation(dReal x_axis, dReal y_axis, dReal z_axis, dReal ang) 44 | { 45 | dQFromAxisAndAngle(q, x_axis, y_axis, z_axis, ang); 46 | isQSet = true; 47 | } 48 | 49 | void PObject::setBodyPosition(dReal x, dReal y, dReal z, bool local) 50 | { 51 | if (!local) 52 | dBodySetPosition(body, x, y, z); 53 | else 54 | { 55 | local_Pos[0] = x; 56 | local_Pos[1] = y; 57 | local_Pos[2] = z; 58 | } 59 | } 60 | 61 | void PObject::setBodyRotation(dReal x_axis, dReal y_axis, dReal z_axis, dReal ang, bool local) 62 | { 63 | if (!local) 64 | { 65 | dQFromAxisAndAngle(q, x_axis, y_axis, z_axis, ang); 66 | dBodySetQuaternion(body, q); 67 | } 68 | else 69 | { 70 | dRFromAxisAndAngle(local_Rot, x_axis, y_axis, z_axis, ang); 71 | } 72 | } 73 | 74 | void PObject::getBodyPosition(dReal &x, dReal &y, dReal &z, bool local) 75 | { 76 | if (local) 77 | { 78 | x = local_Pos[0]; 79 | y = local_Pos[1]; 80 | z = local_Pos[2]; 81 | return; 82 | } 83 | const dReal *r = dBodyGetPosition(body); 84 | x = r[0]; 85 | y = r[1]; 86 | z = r[2]; 87 | } 88 | 89 | void PObject::getBodyDirection(dReal &x, dReal &y, dReal &z) 90 | { 91 | const dReal *r = dBodyGetRotation(body); 92 | dVector3 v = {1, 0, 0}; 93 | dVector3 axis; 94 | dMultiply0(axis, r, v, 4, 3, 1); 95 | x = axis[0]; 96 | y = axis[1]; 97 | z = axis[2]; 98 | } 99 | 100 | void PObject::getBodyDirection(dReal &x, dReal &y, dReal &z, dReal &k) 101 | { 102 | const dReal *r = dBodyGetRotation(body); 103 | dVector3 v = {1, 0, 0}; 104 | dVector3 axis; 105 | dMultiply0(axis, r, v, 4, 3, 1); 106 | x = axis[0]; 107 | y = axis[1]; 108 | z = axis[2]; 109 | k = r[10]; 110 | } 111 | 112 | void PObject::getBodyRotation(dMatrix3 r, bool local) 113 | { 114 | if (local) 115 | { 116 | for (int k = 0; k < 12; k++) 117 | r[k] = local_Rot[k]; 118 | } 119 | else 120 | { 121 | const dReal *rr = dBodyGetRotation(body); 122 | for (int k = 0; k < 12; k++) 123 | r[k] = rr[k]; 124 | } 125 | } 126 | 127 | void PObject::initPosBody() 128 | { 129 | dBodySetPosition(body, m_x, m_y, m_z); 130 | if (isQSet) 131 | dBodySetQuaternion(body, q); 132 | } 133 | 134 | void PObject::initPosGeom() 135 | { 136 | dGeomSetPosition(geom, m_x, m_y, m_z); 137 | if (isQSet) 138 | dGeomSetQuaternion(geom, q); 139 | } 140 | 141 | void PObject::setMass(dReal mass) 142 | { 143 | m_mass = mass; 144 | } 145 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rSim 2 | 3 | Robot soccer simulator based on [FiraSim](https://github.com/fira-simurosot/FIRASim) and [GrSim](https://github.com/RoboCup-SSL/grSim). 4 | 5 | Simulates games from LARC Very Small Size and RoboCup Small Size leagues. 6 | 7 | ## Installing from PyPi 8 | ```shell 9 | $pip install rc-robosim 10 | ``` 11 | Currently wheels are distributed to manylinux2010 for python 3.7-3.10. 12 | If your environment does not have available wheels it will download from source and attempt to build it, which will require having [ODE](https://bitbucket.org/odedevs/ode) version 0.16.2 library installed with libccd collisions 13 | 14 | ## Installing from source 15 | ```shell 16 | $git clone [this repo] 17 | $cd rSim 18 | $pip install . 19 | ``` 20 | This will also require having ODE 0.16.2 with libccd installed 21 | 22 | ## Usage VSS simulator 23 | ```python 24 | import robosim 25 | 26 | field_type = 0 # 0 for Division B, 1 for Division A 27 | n_robots_blue = 2 # number of blue robots 28 | n_robots_yellow = 2 # number of yellow robots 29 | time_step_ms = 25 # time step in milliseconds 30 | # ball initial position [x, y, v_x, v_y] in meters and meter/s 31 | ball_pos = [0.0, 0.0, 0.0, 0.0] 32 | 33 | # robots initial positions [[x, y, angle], [x, y, angle]...], where [[id_0], [id_1]...] 34 | # Units are meters and degrees 35 | blue_robots_pos = [[-0.2, 0.0, 0.0], [-0.4, 0.0, 0.0]] 36 | yellow_robots_pos = [[0.2, 0.0, 0.0], [0.4, 0.0, 0.0]] 37 | 38 | # Init simulator 39 | sim = robosim.VSS( 40 | field_type, 41 | n_robots_blue, 42 | n_robots_yellow, 43 | time_step_ms, 44 | ball_pos, 45 | blue_robots_pos, 46 | yellow_robots_pos, 47 | ) 48 | 49 | # Get dictionary with field parameters 50 | field_params = sim.get_field_params() 51 | 52 | # Get simulator state 53 | # Units are meters, meters/s, degrees 54 | # states is [ball_x, ball_y, ball_z, ball_v_x, ball_v_y, 55 | # blue_0_x, blue_0_y, blue_0_angle, blue_0_v_x, blue_0_v_y, blue_0_v_angle, 56 | # blue_1_x,... 57 | # yellow_0_x, yellow_0_y, yellow_0_angle, yellow_0_v_x, yellow_0_v_y, yellow_0_v_angle, 58 | # yellow_1_x,...] 59 | sim.get_state() 60 | 61 | # Step simulator 62 | # arguments are actions to robots starting from blue robos and increasing ids 63 | # e.g. [[blue_id_0.left_wheel_speed, blue_id_0.right_wheel_speed], 64 | # [blue_id_1.left_wheel_speed, blue_id_1.right_wheel_speed], 65 | # [yellow_id_0.left_wheel_speed, yellow_id_0.right_wheel_speed], 66 | # [yellow_id_1.left_wheel_speed, yellow_id_1.right_wheel_speed]] 67 | # Units are in rad/s 68 | sim.step([[10.0, -10.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0]]) 69 | 70 | # Reset simulator, ball and robots are positioned with same argument format as init 71 | sim.reset(ball_pos, blue_robots_pos, yellow_robots_pos) 72 | 73 | ``` 74 | 75 | 76 | ## Usage SSL simulator 77 | ```python 78 | import robosim 79 | 80 | field_type = 2 # 0 for Division A, 1 for Division B, 2 Hardware Challenges 81 | n_robots_blue = 2 # number of blue robots 82 | n_robots_yellow = 2 # number of yellow robots 83 | time_step_ms = 25 # time step in milliseconds 84 | # ball initial position [x, y, v_x, v_y] in meters and meter/s 85 | ball_pos = [0.0, 0.0, 0.0, 0.0] 86 | 87 | # robots initial positions [[x, y, angle], [x, y, angle]...], where [[id_0], [id_1]...] 88 | # Units are meters and degrees 89 | blue_robots_pos = [[-0.2, 0.0, 0.0], [-0.4, 0.0, 0.0]] 90 | yellow_robots_pos = [[0.2, 0.0, 0.0], [0.4, 0.0, 0.0]] 91 | 92 | # Init simulator 93 | sim = robosim.SSL( 94 | field_type, 95 | n_robots_blue, 96 | n_robots_yellow, 97 | time_step_ms, 98 | ball_pos, 99 | blue_robots_pos, 100 | yellow_robots_pos, 101 | ) 102 | 103 | # Get dictionary with field parameters 104 | field_params = sim.get_field_params() 105 | 106 | # Get simulator state 107 | # Units are meters, meters/s, degrees 108 | # states is [ball_x, ball_y, ball_z, ball_v_x, ball_v_y, 109 | # blue_0_x, blue_0_y, blue_0_angle, blue_0_v_x, blue_0_v_y, blue_0_v_angle, 110 | # blue_0_infrared, blue_0_desired_wheel0_speed, blue_0_desired_wheel1_speed, 111 | # blue_0_desired_wheel2_speed, blue_0_desired_wheel3_speed, ...] 112 | sim.get_state() 113 | 114 | # Step simulator 115 | # arguments are actions to robots starting from blue robos and increasing ids 116 | # e.g. [ 117 | # [blue_id_0.has_v_wheel, 118 | # blue_id_0.wheel_0_speed or v_x, blue_id_0.wheel_1_speed or v_y, blue_id_0.wheel_2_speed or v_angle, blue_id_0.wheel_3_speed, 119 | # blue_id_0.kick_v_x, blue_id_0.kick_v_y, blue_id_0.dribbler 120 | # ], 121 | # ...], 122 | # Units are in rad/s, meters and meters/s and boolean 123 | actions: List[List[float]] = [ 124 | [0.0 for _ in range(6)] for _ in range(n_robots_blue + n_robots_yellow) 125 | ] 126 | sim.step(actions) 127 | 128 | # Reset simulator, ball and robots are positioned with same argument format as init 129 | sim.reset(ball_pos, blue_robots_pos, yellow_robots_pos) 130 | ``` 131 | -------------------------------------------------------------------------------- /src/robosim/vssconfig.h: -------------------------------------------------------------------------------- 1 | #ifndef VSSCONFIG_H 2 | #define VSSCONFIG_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace VSSConfig 9 | { 10 | class Field 11 | { 12 | private: 13 | int RobotsCount = 3; 14 | int RobotsBlueCount = 3; 15 | int RobotsYellowCount = 3; 16 | int FieldType = 0; 17 | double FieldLineWidth = 0.003; 18 | double FieldLength = 1.50; 19 | double FieldWidth = 1.300; 20 | double FieldRad = 0.200; 21 | double FieldFreeKick = 0.200; 22 | double FieldPenaltyWidth = 0.70; 23 | double FieldPenaltyDepth = 0.15; 24 | double FieldPenaltyPoint = 0.35; 25 | double FieldMargin = 0.3; 26 | double FieldRefereeMargin = 0.4; 27 | double WallThickness = 0.025; 28 | double GoalThickness = 0.025; 29 | double GoalDepth = 0.10; 30 | double GoalWidth = 0.40; 31 | double GoalHeight = 0.05; 32 | void setFieldLineWidth(double value) { this->FieldLineWidth = value; } 33 | void setFieldLength(double value) { this->FieldLength = value; } 34 | void setFieldWidth(double value) { this->FieldWidth = value; } 35 | void setFieldRad(double value) { this->FieldRad = value; } 36 | void setFieldFreeKick(double value) { this->FieldFreeKick = value; } 37 | void setFieldPenaltyWidth(double value) { this->FieldPenaltyWidth = value; } 38 | void setFieldPenaltyDepth(double value) { this->FieldPenaltyDepth = value; } 39 | void setFieldPenaltyPoint(double value) { this->FieldPenaltyPoint = value; } 40 | void setFieldMargin(double value) { this->FieldMargin = value; } 41 | void setFieldRefereeMargin(double value) { this->FieldRefereeMargin = value; } 42 | void setWallThickness(double value) { this->WallThickness = value; } 43 | void setGoalThickness(double value) { this->GoalThickness = value; } 44 | void setGoalDepth(double value) { this->GoalDepth = value; } 45 | void setGoalWidth(double value) { this->GoalWidth = value; } 46 | void setGoalHeight(double value) { this->GoalHeight = value; } 47 | 48 | public: 49 | int getRobotsCount() { return this->RobotsCount; } 50 | int getRobotsBlueCount() { return this->RobotsBlueCount; } 51 | int getRobotsYellowCount() { return this->RobotsYellowCount; } 52 | double getFieldLineWidth() { return this->FieldLineWidth; } 53 | double getFieldLength() { return this->FieldLength; } 54 | double getFieldWidth() { return this->FieldWidth; } 55 | double getFieldRad() { return this->FieldRad; } 56 | double getFieldFreeKick() { return this->FieldFreeKick; } 57 | double getFieldPenaltyWidth() { return this->FieldPenaltyWidth; } 58 | double getFieldPenaltyDepth() { return this->FieldPenaltyDepth; } 59 | double getFieldPenaltyPoint() { return this->FieldPenaltyPoint; } 60 | double getFieldMargin() { return this->FieldMargin; } 61 | double getFieldRefereeMargin() { return this->FieldRefereeMargin; } 62 | double getWallThickness() { return this->WallThickness; } 63 | double getGoalThickness() { return this->GoalThickness; } 64 | double getGoalDepth() { return this->GoalDepth; } 65 | double getGoalWidth() { return this->GoalWidth; } 66 | double getGoalHeight() { return this->GoalHeight; } 67 | int getFieldType() { return this->FieldType; } 68 | void setRobotsCount(int value) { this->RobotsCount = value; } 69 | void setRobotsBlueCount(int value) { this->RobotsBlueCount = value; } 70 | void setRobotsYellowCount(int value) { this->RobotsYellowCount = value; } 71 | void setFieldType(int value) 72 | { 73 | this->FieldType = value; 74 | switch (this->FieldType) 75 | { 76 | case 0: // 3x3 77 | setFieldLength(1.50); 78 | setFieldWidth(1.300); 79 | setFieldRad(0.200); 80 | setFieldFreeKick(0.200); 81 | setFieldPenaltyWidth(0.70); 82 | setFieldPenaltyDepth(0.15); 83 | setFieldPenaltyPoint(0.35); 84 | break; 85 | case 1: // 5x5 86 | setFieldLength(2.2); 87 | setFieldWidth(1.8); 88 | setFieldRad(0.25); 89 | setFieldFreeKick(0.25); 90 | setFieldPenaltyWidth(0.8); 91 | setFieldPenaltyDepth(0.35); 92 | setFieldPenaltyPoint(0.375); 93 | break; 94 | default: 95 | break; 96 | } 97 | } 98 | }; 99 | 100 | class World 101 | { 102 | private: 103 | double BallRadius = 0.0215; 104 | int DesiredFPS = 60; 105 | double Gravity = 9.81; 106 | bool ResetTurnOver = true; 107 | double BallMass = 0.043; 108 | double BallFriction = 0.01; 109 | int BallSlip = 1; 110 | double BallBounce = 0.5; 111 | double BallBounceVel = 0.1; 112 | double BallLinearDamp = 0.004; 113 | double BallAngularDamp = 0.004; 114 | 115 | public: 116 | double getBallRadius() { return this->BallRadius; } 117 | int getDesiredFPS() { return this->DesiredFPS; } 118 | double getGravity() { return this->Gravity; } 119 | bool getResetTurnOver() { return this->ResetTurnOver; } 120 | double getBallMass() { return this->BallMass; } 121 | double getBallFriction() { return this->BallFriction; } 122 | int getBallSlip() { return this->BallSlip; } 123 | double getBallBounce() { return this->BallBounce; } 124 | double getBallBounceVel() { return this->BallBounceVel; } 125 | double getBallLinearDamp() { return this->BallLinearDamp; } 126 | double getBallAngularDamp() { return this->BallAngularDamp; } 127 | }; 128 | 129 | class Robot 130 | { 131 | private: 132 | double Radius = 0.0375; 133 | double Height = 0.056; 134 | double BottomHeight = 0.002; 135 | double WheelRadius = 0.026; 136 | double WheelThickness = 0.0025; 137 | int Wheel0Angle = 90; 138 | int Wheel1Angle = 270; 139 | double BodyMass = 0.120; 140 | double WheelMass = 0.015; 141 | double WheelTangentFriction = 0.8; 142 | double WheelPerpendicularFriction = 1; 143 | double WheelMotorMaxTorque = 0.0725; 144 | double WheelMotorMaxRPM = 440.0; 145 | double casterWheelsRadius = 0.002; 146 | double casterWheelsMass = 0.001; 147 | 148 | public: 149 | double getRadius() { return this->Radius; } 150 | double getHeight() { return this->Height; } 151 | double getBottomHeight() { return this->BottomHeight; } 152 | double getWheelRadius() { return this->WheelRadius; } 153 | double getWheelThickness() { return this->WheelThickness; } 154 | int getWheel0Angle() { return this->Wheel0Angle; } 155 | int getWheel1Angle() { return this->Wheel1Angle; } 156 | double getBodyMass() { return this->BodyMass; } 157 | double getWheelMass() { return this->WheelMass; } 158 | double getWheelTangentFriction() { return this->WheelTangentFriction; } 159 | double getWheelPerpendicularFriction() { return this->WheelPerpendicularFriction; } 160 | double getWheelMotorMaxTorque() { return this->WheelMotorMaxTorque; } 161 | double getWheelMotorMaxRPM() { return this->WheelMotorMaxRPM; } 162 | double getCasterWheelsRadius() { return this->casterWheelsRadius; } 163 | double getCasterWheelsMass() { return this->casterWheelsMass; } 164 | 165 | }; 166 | 167 | } // namespace Config 168 | 169 | #endif -------------------------------------------------------------------------------- /src/robosim/sslconfig.h: -------------------------------------------------------------------------------- 1 | #ifndef SSLCONFIG_H 2 | #define SSLCONFIG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace SSLConfig 10 | { 11 | class Field 12 | { 13 | private: 14 | int robotsCount = 22; 15 | int robotsBlueCount = 11; 16 | int robotsYellowCount = 11; 17 | int fieldType = 0; 18 | double fieldLineWidth = 0.01; 19 | double fieldLength = 12.00; 20 | double fieldWidth = 9.00; 21 | double fieldRad = 0.50; 22 | double fieldFreeKick = 0.70; 23 | double fieldPenaltyWidth = 3.60; 24 | double fieldPenaltyDepth = 1.80; 25 | double fieldPenaltyPoint = 8.00; 26 | double fieldMargin = 0.3; 27 | double fieldRefereeMargin = 0.0; 28 | double wallThickness = 0.05; 29 | double goalThickness = 0.02; 30 | double goalDepth = 0.18; 31 | double goalWidth = 1.8; 32 | double goalHeight = 0.16; 33 | void setFieldLineWidth(double value) { this->fieldLineWidth = value; } 34 | void setFieldLength(double value) { this->fieldLength = value; } 35 | void setFieldWidth(double value) { this->fieldWidth = value; } 36 | void setFieldRad(double value) { this->fieldRad = value; } 37 | void setFieldFreeKick(double value) { this->fieldFreeKick = value; } 38 | void setFieldPenaltyWidth(double value) { this->fieldPenaltyWidth = value; } 39 | void setFieldPenaltyDepth(double value) { this->fieldPenaltyDepth = value; } 40 | void setFieldPenaltyPoint(double value) { this->fieldPenaltyPoint = value; } 41 | void setFieldMargin(double value) { this->fieldMargin = value; } 42 | void setFieldRefereeMargin(double value) { this->fieldRefereeMargin = value; } 43 | void setWallThickness(double value) { this->wallThickness = value; } 44 | void setGoalThickness(double value) { this->goalThickness = value; } 45 | void setGoalDepth(double value) { this->goalDepth = value; } 46 | void setGoalWidth(double value) { this->goalWidth = value; } 47 | void setGoalHeight(double value) { this->goalHeight = value; } 48 | 49 | public: 50 | int getRobotsCount() { return this->robotsCount; } 51 | int getRobotsBlueCount() { return this->robotsBlueCount; } 52 | int getRobotsYellowCount() { return this->robotsYellowCount; } 53 | double getFieldLineWidth() { return this->fieldLineWidth; } 54 | double getFieldLength() { return this->fieldLength; } 55 | double getFieldWidth() { return this->fieldWidth; } 56 | double getFieldRad() { return this->fieldRad; } 57 | double getFieldFreeKick() { return this->fieldFreeKick; } 58 | double getFieldPenaltyWidth() { return this->fieldPenaltyWidth; } 59 | double getFieldPenaltyDepth() { return this->fieldPenaltyDepth; } 60 | double getFieldPenaltyPoint() { return this->fieldPenaltyPoint; } 61 | double getFieldMargin() { return this->fieldMargin; } 62 | double getFieldRefereeMargin() { return this->fieldRefereeMargin; } 63 | double getWallThickness() { return this->wallThickness; } 64 | double getGoalThickness() { return this->goalThickness; } 65 | double getGoalDepth() { return this->goalDepth; } 66 | double getGoalWidth() { return this->goalWidth; } 67 | double getGoalHeight() { return this->goalHeight; } 68 | int getFieldType() { return this->fieldType; } 69 | void setRobotsCount(int value) { this->robotsCount = value; } 70 | void setRobotsBlueCount(int value) { this->robotsBlueCount = value; } 71 | void setRobotsYellowCount(int value) { this->robotsYellowCount = value; } 72 | void setFieldType(int value) 73 | { 74 | this->fieldType = value; 75 | switch (this->fieldType) 76 | { 77 | case 0: // Division A 78 | setRobotsCount(22); 79 | setRobotsBlueCount(11); 80 | setRobotsYellowCount(11); 81 | setFieldLength(12.00); 82 | setFieldWidth(9.00); 83 | setFieldPenaltyWidth(3.60); 84 | setFieldPenaltyDepth(1.80); 85 | setFieldPenaltyPoint(8.00); 86 | setGoalWidth(1.80); 87 | break; 88 | case 1: // Division B 89 | setRobotsCount(12); 90 | setRobotsBlueCount(6); 91 | setRobotsYellowCount(6); 92 | setFieldLength(9.00); 93 | setFieldWidth(6.00); 94 | setFieldPenaltyWidth(2.00); 95 | setFieldPenaltyDepth(1.00); 96 | setFieldPenaltyPoint(6.00); 97 | setGoalWidth(1.00); 98 | break; 99 | case 2: // Hardware challenge 100 | setRobotsCount(12); 101 | setRobotsBlueCount(6); 102 | setRobotsYellowCount(6); 103 | setFieldLength(6.00); 104 | setFieldWidth(4.00); 105 | setFieldPenaltyWidth(2.00); 106 | setFieldPenaltyDepth(0.80); 107 | setFieldPenaltyPoint(6.00); 108 | setGoalWidth(0.70); 109 | break; 110 | default: 111 | break; 112 | } 113 | } 114 | }; 115 | 116 | class World 117 | { 118 | private: 119 | int ballSlip = 1; 120 | bool resetTurnOver = true; 121 | double ballRadius = 0.0215; 122 | double gravity = 9.81; 123 | double ballMass = 0.043; 124 | double ballFriction = 0.05; 125 | double ballBounce = 0.5; 126 | double ballBounceVel = 0.1; 127 | double ballLinearDamp = 0.004; 128 | double ballAngularDamp = 0.004; 129 | 130 | public: 131 | int getBallSlip() { return this->ballSlip; } 132 | bool getResetTurnOver() { return this->resetTurnOver; } 133 | double getBallRadius() { return this->ballRadius; } 134 | double getGravity() { return this->gravity; } 135 | double getBallMass() { return this->ballMass; } 136 | double getBallFriction() { return this->ballFriction; } 137 | double getBallBounce() { return this->ballBounce; } 138 | double getBallBounceVel() { return this->ballBounceVel; } 139 | double getBallLinearDamp() { return this->ballLinearDamp; } 140 | double getBallAngularDamp() { return this->ballAngularDamp; } 141 | }; 142 | 143 | class Robot 144 | { 145 | private: 146 | int wheel0Angle = 60; 147 | int wheel1Angle = 135; 148 | int wheel2Angle = 225; 149 | int wheel3Angle = 300; 150 | double radius = 0.090; 151 | double height = 0.146; 152 | double bottomHeight = 0.004; 153 | double wheelRadius = 0.02475; 154 | double wheelThickness = 0.005; 155 | double bodyMass = 2.200; 156 | double wheelMass = 0.050; 157 | double wheelTangentFriction = 0.800; 158 | double wheelPerpendicularFriction = 0.050; 159 | double wheelMotorMaxTorque = 0.070; // Maxon EC45 flat 50w with 18:60 gear ratio, reduced to 3m/s2 accel 160 | double wheelMotorMaxRPM = 1557.0; // Maxon EC45 flat 50w with 18:60 gear ratio 161 | 162 | double distanceCenterKicker = 0.081; 163 | double kickerZ = 0.005; 164 | double kickerThickness = 0.005; 165 | double kickerWidth = 0.080; 166 | double kickerHeight = 0.040; 167 | double kickerMass = 0.020; 168 | double kickerDampFactor = 0.200; 169 | double kickerFriction = 0.800; 170 | double kickerAngle = acos(distanceCenterKicker/radius); 171 | double rollerTorqueFactor = 0.060; 172 | double rollerPerpendicularTorqueFactor = 0.005; 173 | 174 | 175 | public: 176 | int getWheel0Angle() { return this->wheel0Angle; } 177 | int getWheel1Angle() { return this->wheel1Angle; } 178 | int getWheel2Angle() { return this->wheel2Angle; } 179 | int getWheel3Angle() { return this->wheel3Angle; } 180 | double getDistanceCenterKicker() { return this->distanceCenterKicker; } 181 | double getKickerZ() { return this->kickerZ; } 182 | double getKickerThickness() { return this->kickerThickness; } 183 | double getKickerWidth() { return this->kickerWidth; } 184 | double getKickerHeight() { return this->kickerHeight; } 185 | double getKickerMass() { return this->kickerMass; } 186 | double getKickerDampFactor() { return this->kickerDampFactor; } 187 | double getKickerFriction() { return this->kickerFriction; } 188 | double getKickerAngle() { return this->kickerAngle; } 189 | double getDribblerTorqueFactor() { return this->rollerTorqueFactor; } 190 | double getDribblerPerpendicularTorqueFactor() { return this->rollerPerpendicularTorqueFactor; } 191 | double getRadius() { return this->radius; } 192 | double getHeight() { return this->height; } 193 | double getBottomHeight() { return this->bottomHeight; } 194 | double getWheelRadius() { return this->wheelRadius; } 195 | double getWheelThickness() { return this->wheelThickness; } 196 | double getBodyMass() { return this->bodyMass; } 197 | double getWheelMass() { return this->wheelMass; } 198 | double getWheelTangentFriction() { return this->wheelTangentFriction; } 199 | double getWheelPerpendicularFriction() { return this->wheelPerpendicularFriction; } 200 | double getWheelMotorMaxTorque() { return this->wheelMotorMaxTorque; } 201 | double getWheelMotorMaxRPM() { return this->wheelMotorMaxRPM; } 202 | 203 | }; 204 | 205 | } // namespace Config 206 | 207 | #endif -------------------------------------------------------------------------------- /src/robosim/vssrobot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "vssrobot.h" 20 | 21 | // ang2 = position angle 22 | // ang = rotation angle 23 | VSSRobot::Wheel::Wheel(VSSRobot *robot, int _id, dReal ang, dReal ang2) 24 | { 25 | this->id = _id; 26 | this->rob = robot; 27 | dReal rad = VSSConfig::Robot().getRadius() + VSSConfig::Robot().getWheelThickness() / 2.0; 28 | ang *= M_PI / 180.0f; 29 | ang2 *= M_PI / 180.0f; 30 | dReal x = rob->m_x; 31 | dReal y = rob->m_y; 32 | dReal z = rob->m_z; 33 | dReal centerx = x + rad * cos(ang2); 34 | dReal centery = y + rad * sin(ang2); 35 | dReal centerz = z - VSSConfig::Robot().getHeight() * 0.5 + VSSConfig::Robot().getWheelRadius() - VSSConfig::Robot().getBottomHeight(); 36 | this->cyl = new PCylinder(centerx, centery, centerz, VSSConfig::Robot().getWheelRadius(), VSSConfig::Robot().getWheelThickness(), VSSConfig::Robot().getWheelMass()); 37 | this->cyl->setRotation(-sin(ang), cos(ang), 0, M_PI * 0.5); 38 | this->cyl->setBodyRotation(-sin(ang), cos(ang), 0, M_PI * 0.5, true); //set local rotation matrix 39 | this->cyl->setBodyPosition(centerx - x, centery - y, centerz - z, true); //set local position vector 40 | 41 | this->rob->physics->addWheelObject(this->cyl); 42 | 43 | this->joint = dJointCreateHinge(this->rob->physics->world, nullptr); 44 | 45 | dJointAttach(this->joint, this->rob->chassis->body, this->cyl->body); 46 | const dReal *a = dBodyGetPosition(this->cyl->body); 47 | dJointSetHingeAxis(this->joint, cos(ang), sin(ang), 0); 48 | dJointSetHingeAnchor(this->joint, a[0], a[1], a[2]); 49 | 50 | this->motor = dJointCreateAMotor(this->rob->physics->world, nullptr); 51 | dJointAttach(this->motor, this->rob->chassis->body, this->cyl->body); 52 | dJointSetAMotorNumAxes(this->motor, 1); 53 | dJointSetAMotorAxis(this->motor, 0, 1, cos(ang), sin(ang), 0); 54 | dJointSetAMotorParam(this->motor, dParamFMax, VSSConfig::Robot().getWheelMotorMaxTorque()); 55 | this->desiredAngularSpeed = 0; 56 | this->maxAngularSpeed = (VSSConfig::Robot().getWheelMotorMaxRPM() * 2 * M_PI) / 60; 57 | this->desiredAngularSpeed = 0; 58 | } 59 | 60 | VSSRobot::Wheel::~Wheel() { 61 | delete this->cyl; 62 | } 63 | 64 | void VSSRobot::Wheel::step() 65 | { 66 | auto sent_speed = std::max(std::min(this->desiredAngularSpeed, this->maxAngularSpeed), -this->maxAngularSpeed); 67 | dJointSetAMotorParam(this->motor, dParamVel, sent_speed); 68 | dJointSetAMotorParam(this->motor, dParamFMax, VSSConfig::Robot().getWheelMotorMaxTorque()); 69 | } 70 | 71 | VSSRobot::RBall::RBall(VSSRobot *robot, int _id, dReal ang, dReal ang2) 72 | { 73 | id = _id; 74 | rob = robot; 75 | dReal rad = VSSConfig::Robot().getRadius() - VSSConfig::Robot().getCasterWheelsRadius(); 76 | rad = sqrt((rad * rad) + (rad * rad)); 77 | ang *= M_PI / 180.0f; 78 | ang2 *= M_PI / 180.0f; 79 | dReal x = rob->m_x; 80 | dReal y = rob->m_y; 81 | dReal z = rob->m_z; 82 | dReal centerx = x + rad * cos(ang2); 83 | dReal centery = y + rad * sin(ang2); 84 | dReal centerz = z - VSSConfig::Robot().getHeight() * 0.5 + VSSConfig::Robot().getCasterWheelsRadius() - VSSConfig::Robot().getBottomHeight() + 0.0001; 85 | pBall = new PBall(centerx, centery, centerz, VSSConfig::Robot().getCasterWheelsRadius(), VSSConfig::Robot().getCasterWheelsMass()); 86 | pBall->setRotation(-sin(ang), cos(ang), 0, M_PI * 0.5); 87 | pBall->setBodyRotation(-sin(ang), cos(ang), 0, M_PI * 0.5, true); //set local rotation matrix 88 | pBall->setBodyPosition(centerx - x, centery - y, centerz - z, true); //set local position vector 89 | 90 | rob->physics->addWheelObject(pBall); 91 | 92 | joint = dJointCreateHinge(rob->physics->world, nullptr); 93 | 94 | dJointAttach(joint, rob->chassis->body, pBall->body); 95 | const dReal *a = dBodyGetPosition(pBall->body); 96 | dJointSetHingeAxis(joint, cos(ang), sin(ang), 0); 97 | dJointSetHingeAnchor(joint, a[0], a[1], a[2]); 98 | } 99 | 100 | VSSRobot::RBall::~RBall() { 101 | delete this->pBall; 102 | } 103 | 104 | 105 | VSSRobot::VSSRobot(PWorld *world, PBall *ball, dReal x, dReal y, dReal z, 106 | int rob_id, int dir, bool turn_on) 107 | { 108 | m_x = x; 109 | m_y = y; 110 | m_z = z; 111 | physics = world; 112 | m_ball = ball; 113 | m_dir = dir; 114 | m_rob_id = rob_id; 115 | 116 | space = physics->space; 117 | 118 | chassis = new PBox(x, y, z, VSSConfig::Robot().getRadius() * 2, VSSConfig::Robot().getRadius() * 2, VSSConfig::Robot().getHeight(), VSSConfig::Robot().getBodyMass() * 0.99f); 119 | physics->addChassisObject(chassis); 120 | 121 | this->wheels[0] = new Wheel(this, 0, VSSConfig::Robot().getWheel0Angle(), VSSConfig::Robot().getWheel0Angle()); 122 | this->wheels[1] = new Wheel(this, 1, VSSConfig::Robot().getWheel1Angle(), VSSConfig::Robot().getWheel1Angle()); 123 | this->balls[0] = new RBall(this, 0, 45, 45); 124 | this->balls[1] = new RBall(this, 1, -45, -45); 125 | this->balls[2] = new RBall(this, 2, 135, 135); 126 | this->balls[3] = new RBall(this, 3, -135, -135); 127 | setDir(m_dir); 128 | on = turn_on; 129 | } 130 | 131 | VSSRobot::~VSSRobot() 132 | { 133 | delete this->chassis; 134 | for (auto &wheel : this->wheels) delete(wheel); 135 | for (auto &ball : this->balls) delete(ball); 136 | } 137 | 138 | PBall *VSSRobot::getBall() 139 | { 140 | return m_ball; 141 | } 142 | 143 | PWorld *VSSRobot::getWorld() 144 | { 145 | return physics; 146 | } 147 | 148 | int VSSRobot::getID() 149 | { 150 | return m_rob_id - 1; 151 | } 152 | 153 | void normalizeVector(dReal &x, dReal &y, dReal &z) 154 | { 155 | dReal d = sqrt(x * x + y * y + z * z); 156 | x /= d; 157 | y /= d; 158 | z /= d; 159 | } 160 | 161 | void VSSRobot::step() 162 | { 163 | if (!on) 164 | { 165 | if (last_state) 166 | wheels[0]->desiredAngularSpeed = wheels[1]->desiredAngularSpeed = 0; 167 | } 168 | for (auto &wheel : wheels) 169 | wheel->step(); 170 | last_state = on; 171 | } 172 | 173 | void VSSRobot::resetSpeeds() 174 | { 175 | wheels[0]->desiredAngularSpeed = wheels[1]->desiredAngularSpeed = 0; 176 | } 177 | 178 | void VSSRobot::resetRobot() 179 | { 180 | resetSpeeds(); 181 | dBodySetLinearVel(chassis->body, 0, 0, 0); 182 | dBodySetAngularVel(chassis->body, 0, 0, 0); 183 | for (auto &wheel : wheels) 184 | { 185 | dBodySetLinearVel(wheel->cyl->body, 0, 0, 0); 186 | dBodySetAngularVel(wheel->cyl->body, 0, 0, 0); 187 | } 188 | dReal x, y; 189 | getXY(x, y); 190 | setXY(x, y); 191 | setDir(m_dir); 192 | } 193 | 194 | void VSSRobot::getXY(dReal &x, dReal &y) 195 | { 196 | dReal xx, yy, zz; 197 | chassis->getBodyPosition(xx, yy, zz); 198 | x = xx; 199 | y = yy; 200 | } 201 | 202 | dReal VSSRobot::getDir(dReal &k) 203 | { 204 | dReal x, y, z; 205 | chassis->getBodyDirection(x, y, z, k); 206 | dReal dot = x; //zarb dar (1.0,0.0,0.0) 207 | dReal length = sqrt(x * x + y * y); 208 | auto absAng = (dReal)(acos((dReal)(dot / length)) * (180.0f / M_PI)); 209 | return (y > 0) ? absAng : 360-absAng; 210 | } 211 | 212 | void VSSRobot::setXY(dReal x, dReal y) 213 | { 214 | dReal xx, yy, zz, kx, ky, kz; 215 | dReal height = VSS_ROBOT_START_Z(); 216 | chassis->getBodyPosition(xx, yy, zz); 217 | chassis->setBodyPosition(x, y, height); 218 | for (auto &wheel : wheels) 219 | { 220 | wheel->cyl->getBodyPosition(kx, ky, kz); 221 | wheel->cyl->setBodyPosition(kx - xx + x, ky - yy + y, kz - zz + height); 222 | } 223 | } 224 | 225 | void VSSRobot::setDir(dReal ang) 226 | { 227 | ang *= M_PI / 180.0f; 228 | chassis->setBodyRotation(0, 0, 1, ang); 229 | dMatrix3 wLocalRot, wRot, cRot; 230 | dVector3 localPos, finalPos, cPos; 231 | chassis->getBodyPosition(cPos[0], cPos[1], cPos[2], false); 232 | chassis->getBodyRotation(cRot, false); 233 | dMultiply0(finalPos, cRot, localPos, 4, 3, 1); 234 | finalPos[0] += cPos[0]; 235 | finalPos[1] += cPos[1]; 236 | finalPos[2] += cPos[2]; 237 | for (auto &wheel : wheels) 238 | { 239 | wheel->cyl->getBodyRotation(wLocalRot, true); 240 | dMultiply0(wRot, cRot, wLocalRot, 3, 3, 3); 241 | dBodySetRotation(wheel->cyl->body, wRot); 242 | wheel->cyl->getBodyPosition(localPos[0], localPos[1], localPos[2], true); 243 | dMultiply0(finalPos, cRot, localPos, 4, 3, 1); 244 | finalPos[0] += cPos[0]; 245 | finalPos[1] += cPos[1]; 246 | finalPos[2] += cPos[2]; 247 | wheel->cyl->setBodyPosition(finalPos[0], finalPos[1], finalPos[2], false); 248 | } 249 | } 250 | 251 | void VSSRobot::setWheelDesiredAngularSpeed(int i, dReal s) 252 | { 253 | if (!((i >= 2) || (i < 0))) 254 | wheels[i]->desiredAngularSpeed = s; 255 | } 256 | 257 | dReal VSSRobot::getSpeed(int i) 258 | { 259 | if ((i >= 2) || (i < 0)) 260 | return -1; 261 | return wheels[i]->desiredAngularSpeed; 262 | } 263 | 264 | void VSSRobot::incSpeed(int i, dReal v) 265 | { 266 | if (!((i >= 2) || (i < 0))) 267 | wheels[i]->desiredAngularSpeed += v; 268 | } 269 | -------------------------------------------------------------------------------- /src/robosim/physics/pworld.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "pworld.h" 20 | #include 21 | 22 | PSurface::PSurface() 23 | { 24 | callback = nullptr; 25 | usefdir1 = false; 26 | surface.mode = dContactApprox1; 27 | surface.mu = 0.5; 28 | } 29 | bool PSurface::isIt(dGeomID i1, dGeomID i2) 30 | { 31 | return ((i1 == id1) && (i2 == id2)) || ((i1 == id2) && (i2 == id1)); 32 | } 33 | 34 | void nearCallback(void *data, dGeomID o1, dGeomID o2) 35 | { 36 | ((PWorld *)data)->handleCollisions(o1, o2); 37 | } 38 | 39 | PWorld::PWorld(dReal dt, dReal gravity, int _robot_count) 40 | { 41 | robot_count = _robot_count; 42 | dInitODE2(0); 43 | dAllocateODEDataForThread(dAllocateMaskAll); 44 | this->world = dWorldCreate(); 45 | // this->space = dHashSpaceCreate(nullptr); 46 | this->space = dSimpleSpaceCreate(nullptr); 47 | this->spaceChassis = dSimpleSpaceCreate(nullptr); 48 | this->spaceKicker= dSimpleSpaceCreate(nullptr); 49 | this->spaceWall= dSimpleSpaceCreate(nullptr); 50 | this->spaceWheel= dSimpleSpaceCreate(nullptr); 51 | this->contactgroup = dJointGroupCreate(0); 52 | dWorldSetGravity(this->world, 0, 0, -gravity); 53 | this->objects_count = 0; 54 | this->sur_matrix = nullptr; 55 | //dAllocateODEDataForThread(dAllocateMaskAll); 56 | delta_time = dt; 57 | } 58 | 59 | PWorld::~PWorld() 60 | { 61 | for(auto x: this->surfaces) 62 | delete x; 63 | for (int i = 0; i < this->objects_count; i++) 64 | delete[] sur_matrix[i]; 65 | delete[] sur_matrix; 66 | dJointGroupDestroy(contactgroup); 67 | dSpaceDestroy(space); 68 | dSpaceDestroy(spaceChassis); 69 | dSpaceDestroy(spaceKicker); 70 | dSpaceDestroy(spaceWall); 71 | dSpaceDestroy(spaceWheel); 72 | dWorldDestroy(world); 73 | dCloseODE(); 74 | } 75 | 76 | void PWorld::setGravity(dReal gravity) 77 | { 78 | dWorldSetGravity(world, 0, 0, -gravity); 79 | } 80 | 81 | void PWorld::handleCollisions(dGeomID o1, dGeomID o2) 82 | { 83 | PSurface *sur; 84 | int j = sur_matrix[*((int *)(dGeomGetData(o1)))][*((int *)(dGeomGetData(o2)))]; 85 | if (j != -1) 86 | { 87 | const int N = 10; 88 | dContact contact[N]; 89 | int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact)); 90 | if (n > 0) 91 | { 92 | sur = surfaces[j]; 93 | sur->contactPos[0] = contact[0].geom.pos[0]; 94 | sur->contactPos[1] = contact[0].geom.pos[1]; 95 | sur->contactPos[2] = contact[0].geom.pos[2]; 96 | sur->contactNormal[0] = contact[0].geom.normal[0]; 97 | sur->contactNormal[1] = contact[0].geom.normal[1]; 98 | sur->contactNormal[2] = contact[0].geom.normal[2]; 99 | bool flag = true; 100 | if (sur->callback != nullptr) 101 | flag = sur->callback(o1, o2, sur, robot_count); 102 | if (flag) 103 | for (int i = 0; i < n; i++) 104 | { 105 | contact[i].surface = sur->surface; 106 | if (sur->usefdir1) 107 | { 108 | contact[i].fdir1[0] = sur->fdir1[0]; 109 | contact[i].fdir1[1] = sur->fdir1[1]; 110 | contact[i].fdir1[2] = sur->fdir1[2]; 111 | contact[i].fdir1[3] = sur->fdir1[3]; 112 | } 113 | dJointID c = dJointCreateContact(world, contactgroup, &contact[i]); 114 | 115 | dJointAttach(c, 116 | dGeomGetBody(contact[i].geom.g1), 117 | dGeomGetBody(contact[i].geom.g2)); 118 | } 119 | } 120 | } 121 | } 122 | 123 | int PWorld::addObject(PObject* o) 124 | { 125 | int id = this->objects.size(); 126 | o->id = id; 127 | if (o->world == nullptr) 128 | o->world = this->world; 129 | if (o->space == nullptr) 130 | o->space = this->space; 131 | o->init(); 132 | dGeomSetData(o->geom, (void *)(&(o->id))); 133 | this->objects.push_back(o); 134 | this->ball = o; 135 | return id; 136 | } 137 | 138 | int PWorld::addBallObject(PObject* o) 139 | { 140 | int id = this->objects.size(); 141 | o->id = id; 142 | if (o->world == nullptr) 143 | o->world = this->world; 144 | if (o->space == nullptr) 145 | o->space = this->space; 146 | o->init(); 147 | dGeomSetData(o->geom, (void *)(&(o->id))); 148 | this->objects.push_back(o); 149 | this->ball = o; 150 | return id; 151 | } 152 | 153 | int PWorld::addGroundObject(PObject* o) 154 | { 155 | int id = this->objects.size(); 156 | o->id = id; 157 | if (o->world == nullptr) 158 | o->world = this->world; 159 | if (o->space == nullptr) 160 | o->space = this->space; 161 | o->init(); 162 | dGeomSetData(o->geom, (void *)(&(o->id))); 163 | this->objects.push_back(o); 164 | this->ground = o; 165 | return id; 166 | } 167 | 168 | int PWorld::addWallObject(PObject* o) 169 | { 170 | int id = this->objects.size(); 171 | o->id = id; 172 | if (o->world == nullptr) 173 | o->world = this->world; 174 | if (o->space == nullptr) 175 | o->space = this->spaceWall; 176 | o->init(); 177 | dGeomSetData(o->geom, (void *)(&(o->id))); 178 | this->objects.push_back(o); 179 | return id; 180 | } 181 | 182 | int PWorld::addWheelObject(PObject* o) 183 | { 184 | int id = this->objects.size(); 185 | o->id = id; 186 | if (o->world == nullptr) 187 | o->world = this->world; 188 | if (o->space == nullptr) 189 | o->space = this->spaceWheel; 190 | o->init(); 191 | dGeomSetData(o->geom, (void *)(&(o->id))); 192 | this->objects.push_back(o); 193 | return id; 194 | } 195 | 196 | int PWorld::addChassisObject(PObject* o) 197 | { 198 | int id = this->objects.size(); 199 | o->id = id; 200 | if (o->world == nullptr) 201 | o->world = this->world; 202 | if (o->space == nullptr) 203 | o->space = this->spaceChassis; 204 | o->init(); 205 | dGeomSetData(o->geom, (void *)(&(o->id))); 206 | this->objects.push_back(o); 207 | return id; 208 | } 209 | 210 | int PWorld::addKickerObject(PObject* o) 211 | { 212 | int id = this->objects.size(); 213 | o->id = id; 214 | if (o->world == nullptr) 215 | o->world = this->world; 216 | if (o->space == nullptr) 217 | o->space = this->spaceKicker; 218 | o->init(); 219 | dGeomSetData(o->geom, (void *)(&(o->id))); 220 | this->objects.push_back(o); 221 | return id; 222 | } 223 | 224 | void PWorld::initAllObjects() 225 | { 226 | objects_count = this->objects.size(); 227 | int c = objects_count; 228 | bool flag = false; 229 | if (sur_matrix != nullptr) 230 | { 231 | for (int i = 0; i < c; i++) 232 | delete sur_matrix[i]; 233 | delete sur_matrix; 234 | flag = true; 235 | } 236 | sur_matrix = new int *[c]; 237 | for (int i = 0; i < c; i++) 238 | { 239 | sur_matrix[i] = new int[c]; 240 | for (int j = 0; j < c; j++) 241 | sur_matrix[i][j] = -1; 242 | } 243 | if (flag) 244 | { 245 | for (int i = 0; i < surfaces.size(); i++) 246 | sur_matrix[(*(int *)(dGeomGetData(surfaces[i]->id1)))][*((int *)(dGeomGetData(surfaces[i]->id2)))] = 247 | sur_matrix[(*(int *)(dGeomGetData(surfaces[i]->id2)))][*((int *)(dGeomGetData(surfaces[i]->id1)))] = i; 248 | } 249 | } 250 | 251 | PSurface *PWorld::createSurface(PObject *o1, PObject *o2) 252 | { 253 | auto *s = new PSurface(); 254 | s->id1 = o1->geom; 255 | s->id2 = o2->geom; 256 | this->surfaces.push_back(s); 257 | this->sur_matrix[o1->id][o2->id] = 258 | this->sur_matrix[o2->id][o1->id] = this->surfaces.size() - 1; 259 | return s; 260 | } 261 | 262 | PSurface* PWorld::createOneWaySurface(PObject* o1,PObject* o2) 263 | { 264 | PSurface *s = new PSurface(); 265 | s->id1 = o1->geom; 266 | s->id2 = o2->geom; 267 | this->surfaces.push_back(s); 268 | this->sur_matrix[o1->id][o2->id] = this->surfaces.size() - 1; 269 | return s; 270 | } 271 | 272 | PSurface *PWorld::findSurface(PObject *o1, PObject *o2) 273 | { 274 | for (int i = 0; i < surfaces.size(); i++) 275 | { 276 | if (surfaces[i]->isIt(o1->geom, o2->geom)) 277 | return (surfaces[i]); 278 | } 279 | return nullptr; 280 | } 281 | 282 | void PWorld::step(dReal dt, bool sync) 283 | { 284 | try 285 | { 286 | // Collide wheels with ground 287 | dSpaceCollide2((dGeomID)this->spaceWheel, this->ground->geom, this, &nearCallback); 288 | 289 | // Collide Ball with ground 290 | dSpaceCollide2(this->ball->geom, this->ground->geom, this, &nearCallback); 291 | 292 | // Collide ball with kicker 293 | dSpaceCollide2(this->ball->geom, (dGeomID)this->spaceKicker, this, &nearCallback); 294 | 295 | // Collide ball with chassis 296 | dSpaceCollide2((dGeomID)this->spaceChassis, this->ball->geom, this, &nearCallback); 297 | 298 | // Collide ball with wall 299 | dSpaceCollide2(this->ball->geom, (dGeomID)this->spaceWall, this, &nearCallback); 300 | 301 | // Collide chassis with wall 302 | dSpaceCollide2((dGeomID)this->spaceChassis, (dGeomID)this->spaceWall, this, &nearCallback); 303 | 304 | // Collide chassis with chassis 305 | dSpaceCollide(this->spaceChassis, this, &nearCallback); 306 | 307 | dWorldSetQuickStepNumIterations(world, 20); 308 | if (sync) 309 | dWorldQuickStep(world, (dt < 0) ? delta_time : dt); 310 | else 311 | dWorldStep(world, (dt < 0) ? delta_time : dt); 312 | dJointGroupEmpty(contactgroup); 313 | } 314 | catch (...) 315 | { 316 | std::cout << "step error\n"; 317 | } 318 | } 319 | -------------------------------------------------------------------------------- /src/robosim/sslrobot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "sslrobot.h" 20 | 21 | // ang2 = position angle 22 | // ang = rotation angle 23 | SSLRobot::Wheel::Wheel(SSLRobot *robot, int _id, dReal ang, dReal ang2) 24 | { 25 | this->id = _id; 26 | this->rob = robot; 27 | dReal rad = SSLConfig::Robot().getRadius() + SSLConfig::Robot().getWheelThickness() / 2.0; 28 | ang *= M_PI / 180.0f; 29 | ang2 *= M_PI / 180.0f; 30 | dReal x = this->rob->_x; 31 | dReal y = this->rob->_y; 32 | dReal z = this->rob->_z; 33 | dReal centerx = x + rad * cos(ang2); 34 | dReal centery = y + rad * sin(ang2); 35 | dReal centerz = z - SSLConfig::Robot().getHeight() * 0.5 + SSLConfig::Robot().getWheelRadius() - SSLConfig::Robot().getBottomHeight(); 36 | this->cyl = new PCylinder(centerx, centery, centerz, SSLConfig::Robot().getWheelRadius(), SSLConfig::Robot().getWheelThickness(), SSLConfig::Robot().getWheelMass()); 37 | this->cyl->setRotation(-sin(ang), cos(ang), 0, M_PI * 0.5); 38 | this->cyl->setBodyRotation(-sin(ang), cos(ang), 0, M_PI * 0.5, true); //set local rotation matrix 39 | this->cyl->setBodyPosition(centerx - x, centery - y, centerz - z, true); //set local position vector 40 | 41 | this->rob->physics->addWheelObject(this->cyl); 42 | 43 | this->joint = dJointCreateHinge(this->rob->physics->world, nullptr); 44 | 45 | dJointAttach(this->joint, this->rob->chassis->body, this->cyl->body); 46 | const dReal *a = dBodyGetPosition(this->cyl->body); 47 | dJointSetHingeAxis(this->joint, cos(ang), sin(ang), 0); 48 | dJointSetHingeAnchor(this->joint, a[0], a[1], a[2]); 49 | 50 | this->motor = dJointCreateAMotor(this->rob->physics->world, nullptr); 51 | dJointAttach(this->motor, this->rob->chassis->body, this->cyl->body); 52 | dJointSetAMotorNumAxes(this->motor, 1); 53 | dJointSetAMotorAxis(this->motor, 0, 1, cos(ang), sin(ang), 0); 54 | dJointSetAMotorParam(this->motor, dParamFMax, SSLConfig::Robot().getWheelMotorMaxTorque()); 55 | this->maxAngularSpeed = (SSLConfig::Robot().getWheelMotorMaxRPM() * 2 * M_PI) / 60; 56 | this->desiredAngularSpeed = 0; 57 | } 58 | 59 | SSLRobot::Wheel::~Wheel() { 60 | delete this->cyl; 61 | } 62 | 63 | void SSLRobot::Wheel::step() 64 | { 65 | auto sent_speed = std::max(std::min(this->desiredAngularSpeed, this->maxAngularSpeed), -this->maxAngularSpeed); 66 | dJointSetAMotorParam(this->motor, dParamVel, sent_speed); 67 | dJointSetAMotorParam(this->motor, dParamFMax, SSLConfig::Robot().getWheelMotorMaxTorque()); 68 | } 69 | 70 | SSLRobot::Kicker::Kicker(SSLRobot* robot) : holdingBall(false) 71 | { 72 | this->rob = robot; 73 | 74 | dReal x = this->rob->_x; 75 | dReal y = this->rob->_y; 76 | dReal z = this->rob->_z; 77 | 78 | dReal centerX = x + (SSLConfig::Robot().getDistanceCenterKicker() - SSLConfig::Robot().getKickerThickness()*0.5); 79 | dReal centerY = y; 80 | dReal centerZ = z - (SSLConfig::Robot().getHeight()) * 0.5f - SSLConfig::Robot().getBottomHeight() + SSLConfig::Robot().getKickerZ() + SSLConfig::Robot().getKickerHeight() * 0.5; 81 | 82 | this->box = new PBox( 83 | centerX, centerY, centerZ, 84 | SSLConfig::Robot().getKickerThickness(), SSLConfig::Robot().getKickerWidth(), 85 | SSLConfig::Robot().getKickerHeight(),SSLConfig::Robot().getKickerMass() 86 | ); 87 | this->box->setBodyPosition(centerX - x, centerY - y, centerZ - z, true); 88 | 89 | this->rob->physics->addKickerObject(this->box); 90 | 91 | this->joint = dJointCreateHinge(this->rob->physics->world, 0); 92 | dJointAttach(this->joint,this->rob->chassis->body,this->box->body); 93 | 94 | const dReal *aa = dBodyGetPosition(this->box->body); 95 | dJointSetHingeAnchor(this->joint, aa[0], aa[1], aa[2]); 96 | dJointSetHingeAxis(this->joint, 0, -1,0); 97 | 98 | dJointSetHingeParam(this->joint,dParamVel,0); 99 | dJointSetHingeParam(this->joint,dParamLoStop,0); 100 | dJointSetHingeParam(this->joint,dParamHiStop,0); 101 | 102 | this->dribblerOn = false; 103 | this->kickerState = NO_KICK; 104 | } 105 | 106 | SSLRobot::Kicker::~Kicker() { 107 | delete this->box; 108 | } 109 | 110 | void SSLRobot::Kicker::step() 111 | { 112 | if (!isTouchingBall() || !this->dribblerOn) unholdBall(); 113 | if (this->kickerState != NO_KICK) 114 | { 115 | this->kickerCounter--; 116 | if (this->kickerCounter<=0) this->kickerState = NO_KICK; 117 | } 118 | else if (this->dribblerOn) 119 | { 120 | if (isTouchingBall()) 121 | { 122 | holdBall(); 123 | } 124 | } 125 | } 126 | 127 | bool SSLRobot::Kicker::isTouchingBall() 128 | { 129 | dReal vx,vy,vz; 130 | dReal bx,by,bz; 131 | dReal kx,ky,kz; 132 | 133 | this->rob->chassis->getBodyDirection(vx,vy,vz); 134 | this->rob->getBall()->getBodyPosition(bx,by,bz); 135 | this->box->getBodyPosition(kx,ky,kz); 136 | 137 | kx += vx * SSLConfig::Robot().getKickerThickness()*0.5f; 138 | ky += vy * SSLConfig::Robot().getKickerThickness()*0.5f; 139 | 140 | dReal xx = fabs((kx-bx)*vx + (ky-by)*vy); 141 | dReal yy = fabs(-(kx-bx)*vy + (ky-by)*vx); 142 | dReal zz = fabs(kz-bz); 143 | 144 | return ((xx < SSLConfig::Robot().getKickerThickness() * 2.0f + SSLConfig::World().getBallRadius()) && (yy < SSLConfig::Robot().getKickerWidth()*0.5f) && (zz < SSLConfig::Robot().getKickerHeight() * 0.5f)); 145 | } 146 | 147 | KickStatus SSLRobot::Kicker::getKickerStatus() 148 | { 149 | return this->kickerState; 150 | } 151 | 152 | void SSLRobot::Kicker::setDribbler(bool dribbler) 153 | { 154 | this->dribblerOn = dribbler; 155 | } 156 | 157 | bool SSLRobot::Kicker::getDribbler() 158 | { 159 | return this->dribblerOn; 160 | } 161 | 162 | void SSLRobot::Kicker::toggleDribbler() 163 | { 164 | this->dribblerOn != this->dribblerOn; 165 | } 166 | 167 | void SSLRobot::Kicker::kick(dReal kickSpeedX, dReal kickSpeedZ) 168 | { 169 | dReal dx,dy,dz; 170 | dReal vx,vy,vz; 171 | 172 | this->rob->chassis->getBodyDirection(dx,dy,dz); 173 | dz = 0; 174 | 175 | unholdBall(); 176 | 177 | if (isTouchingBall()) 178 | { 179 | dReal dlen = dx*dx + dy*dy + dz*dz; 180 | dlen = sqrt(dlen); 181 | 182 | vx = dx*kickSpeedX/dlen; 183 | vy = dy*kickSpeedX/dlen; 184 | vz = kickSpeedZ; 185 | 186 | const dReal* vball = dBodyGetLinearVel(rob->getBall()->body); 187 | dReal vn = -(vball[0]*dx + vball[1]*dy) * SSLConfig::Robot().getKickerDampFactor(); 188 | dReal vt = -(vball[0]*dy - vball[1]*dx); 189 | vx += vn * dx - vt * dy; 190 | vy += vn * dy + vt * dx; 191 | dBodySetLinearVel(this->rob->getBall()->body,vx,vy,vz); 192 | 193 | if (kickSpeedZ >= 1) 194 | this->kickerState = CHIP_KICK; 195 | else 196 | this->kickerState = FLAT_KICK; 197 | 198 | this->kickerCounter = 10; 199 | } 200 | } 201 | 202 | void SSLRobot::Kicker::holdBall(){ 203 | dReal vx,vy,vz; 204 | dReal bx,by,bz; 205 | dReal kx,ky,kz; 206 | 207 | this->rob->chassis->getBodyDirection(vx,vy,vz); 208 | this->rob->getBall()->getBodyPosition(bx,by,bz); 209 | this->box->getBodyPosition(kx,ky,kz); 210 | 211 | kx += vx * SSLConfig::Robot().getKickerThickness()*0.5f; 212 | ky += vy * SSLConfig::Robot().getKickerThickness()*0.5f; 213 | 214 | dReal xx = fabs((kx-bx)*vx + (ky-by)*vy); 215 | dReal yy = fabs(-(kx-bx)*vy + (ky-by)*vx); 216 | 217 | if(this->holdingBall || xx - SSLConfig::World().getBallRadius() < 0) return; 218 | dBodySetLinearVel(this->rob->getBall()->body,0,0,0); 219 | this->robot_to_ball = dJointCreateHinge(this->rob->getWorld()->world, 0); 220 | dJointAttach(this->robot_to_ball, this->box->body, this->rob->getBall()->body); 221 | this->holdingBall = true; 222 | } 223 | 224 | void SSLRobot::Kicker::unholdBall(){ 225 | if(this->holdingBall) { 226 | dJointDestroy(this->robot_to_ball); 227 | this->holdingBall = false; 228 | } 229 | } 230 | 231 | SSLRobot::SSLRobot(PWorld *world, PBall *ball, dReal x, dReal y, dReal z, 232 | int robot_id, dReal dir) 233 | { 234 | this->_x = x; 235 | this->_y = y; 236 | this->_z = z; 237 | this->physics = world; 238 | this->ball = ball; 239 | this->_dir = dir; 240 | this->rob_id = robot_id; 241 | 242 | this->chassis = new PCylinder(this->_x, this->_y, this->_z, SSLConfig::Robot().getRadius(), SSLConfig::Robot().getHeight(), SSLConfig::Robot().getBodyMass()); 243 | this->physics->addChassisObject(chassis); 244 | 245 | this->kicker = new Kicker(this); 246 | 247 | this->wheels[0] = new Wheel(this, 0, SSLConfig::Robot().getWheel0Angle(), SSLConfig::Robot().getWheel0Angle()); 248 | this->wheels[1] = new Wheel(this, 1, SSLConfig::Robot().getWheel1Angle(), SSLConfig::Robot().getWheel1Angle()); 249 | this->wheels[2] = new Wheel(this, 2, SSLConfig::Robot().getWheel2Angle(), SSLConfig::Robot().getWheel2Angle()); 250 | this->wheels[3] = new Wheel(this, 3, SSLConfig::Robot().getWheel3Angle(), SSLConfig::Robot().getWheel3Angle()); 251 | 252 | setDir(this->_dir); 253 | } 254 | 255 | SSLRobot::~SSLRobot() { 256 | delete this->chassis; 257 | delete this->kicker; 258 | for (auto &wheel : this->wheels) delete(wheel); 259 | } 260 | 261 | PBall *SSLRobot::getBall() 262 | { 263 | return this->ball; 264 | } 265 | 266 | PWorld *SSLRobot::getWorld() 267 | { 268 | return this->physics; 269 | } 270 | 271 | int SSLRobot::getID() 272 | { 273 | return this->rob_id - 1; 274 | } 275 | 276 | void SSLRobot::step() 277 | { 278 | for (auto &wheel : this->wheels) 279 | wheel->step(); 280 | this->kicker->step(); 281 | } 282 | 283 | void SSLRobot::resetSpeeds() 284 | { 285 | for (auto &wheel : this->wheels) 286 | wheel->desiredAngularSpeed = 0; 287 | } 288 | 289 | void SSLRobot::resetRobot() 290 | { 291 | resetSpeeds(); 292 | dBodySetLinearVel(this->chassis->body, 0, 0, 0); 293 | dBodySetAngularVel(this->chassis->body, 0, 0, 0); 294 | for (auto &wheel : this->wheels) 295 | { 296 | dBodySetLinearVel(wheel->cyl->body, 0, 0, 0); 297 | dBodySetAngularVel(wheel->cyl->body, 0, 0, 0); 298 | } 299 | dReal x, y; 300 | getXY(x, y); 301 | setXY(x, y); 302 | setDir(this->_dir); 303 | } 304 | 305 | void SSLRobot::getXY(dReal &x, dReal &y) 306 | { 307 | dReal xx, yy, zz; 308 | this->chassis->getBodyPosition(xx, yy, zz); 309 | x = xx; 310 | y = yy; 311 | } 312 | 313 | dReal SSLRobot::getDir(dReal &k) 314 | { 315 | dReal x, y, z; 316 | this->chassis->getBodyDirection(x, y, z, k); 317 | 318 | dReal dot = x; //zarb dar (1.0,0.0,0.0) 319 | dReal length = sqrt(x * x + y * y); 320 | auto absAng = (dReal)(acos((dReal)(dot / length)) * (180.0f / M_PI)); 321 | 322 | return (y > 0) ? absAng : 360-absAng; 323 | } 324 | 325 | void SSLRobot::setXY(dReal x, dReal y) 326 | { 327 | dReal xx, yy, zz, kx, ky, kz; 328 | dReal height = SSL_ROBOT_START_Z(); 329 | this->chassis->getBodyPosition(xx, yy, zz); 330 | this->chassis->setBodyPosition(x, y, height); 331 | this->kicker->box->getBodyPosition(kx,ky,kz); 332 | this->kicker->box->setBodyPosition(kx-xx+x,ky-yy+y,kz-zz+height); 333 | 334 | for (auto &wheel : this->wheels) 335 | { 336 | wheel->cyl->getBodyPosition(kx, ky, kz); 337 | wheel->cyl->setBodyPosition(kx - xx + x, ky - yy + y, kz - zz + height); 338 | } 339 | } 340 | 341 | void SSLRobot::setDir(dReal ang) 342 | { 343 | dMatrix3 wLocalRot, wRot, cRot; 344 | dVector3 localPos, finalPos, cPos; 345 | ang *= M_PI / 180.0f; 346 | 347 | this->chassis->setBodyRotation(0, 0, 1, ang); 348 | this->kicker->box->setBodyRotation(0,0,1,ang); 349 | 350 | this->chassis->getBodyPosition(cPos[0], cPos[1], cPos[2], false); 351 | this->chassis->getBodyRotation(cRot, false); 352 | dMultiply0(finalPos, cRot, localPos, 4, 3, 1); 353 | finalPos[0] += cPos[0]; 354 | finalPos[1] += cPos[1]; 355 | finalPos[2] += cPos[2]; 356 | for (auto &wheel : this->wheels) 357 | { 358 | wheel->cyl->getBodyRotation(wLocalRot, true); 359 | dMultiply0(wRot, cRot, wLocalRot, 3, 3, 3); 360 | dBodySetRotation(wheel->cyl->body, wRot); 361 | wheel->cyl->getBodyPosition(localPos[0], localPos[1], localPos[2], true); 362 | dMultiply0(finalPos, cRot, localPos, 4, 3, 1); 363 | finalPos[0] += cPos[0]; 364 | finalPos[1] += cPos[1]; 365 | finalPos[2] += cPos[2]; 366 | wheel->cyl->setBodyPosition(finalPos[0], finalPos[1], finalPos[2], false); 367 | } 368 | } 369 | 370 | void SSLRobot::setWheelDesiredAngularSpeed(int i, dReal s) 371 | { 372 | if (!((i >= 4) || (i < 0))) 373 | this->wheels[i]->desiredAngularSpeed = s; 374 | } 375 | 376 | void SSLRobot::setDesiredSpeedLocal(dReal vx, dReal vy, dReal vw) 377 | { 378 | // Calculate Motor Speeds 379 | dReal _DEG2RAD = M_PI / 180.0; 380 | dReal motorAlpha[4] = {SSLConfig::Robot().getWheel0Angle() * _DEG2RAD, SSLConfig::Robot().getWheel1Angle() * _DEG2RAD, SSLConfig::Robot().getWheel2Angle() * _DEG2RAD, SSLConfig::Robot().getWheel3Angle() * _DEG2RAD}; 381 | 382 | // Convert local robot speed to rad/s 383 | dReal dw0 = (1.0 / SSLConfig::Robot().getWheelRadius()) * (( (SSLConfig::Robot().getRadius() * vw) - (vx * sin(motorAlpha[0])) + (vy * cos(motorAlpha[0]))) ); 384 | dReal dw1 = (1.0 / SSLConfig::Robot().getWheelRadius()) * (( (SSLConfig::Robot().getRadius() * vw) - (vx * sin(motorAlpha[1])) + (vy * cos(motorAlpha[1]))) ); 385 | dReal dw2 = (1.0 / SSLConfig::Robot().getWheelRadius()) * (( (SSLConfig::Robot().getRadius() * vw) - (vx * sin(motorAlpha[2])) + (vy * cos(motorAlpha[2]))) ); 386 | dReal dw3 = (1.0 / SSLConfig::Robot().getWheelRadius()) * (( (SSLConfig::Robot().getRadius() * vw) - (vx * sin(motorAlpha[3])) + (vy * cos(motorAlpha[3]))) ); 387 | 388 | setWheelDesiredAngularSpeed(0 , dw0); 389 | setWheelDesiredAngularSpeed(1 , dw1); 390 | setWheelDesiredAngularSpeed(2 , dw2); 391 | setWheelDesiredAngularSpeed(3 , dw3); 392 | } 393 | -------------------------------------------------------------------------------- /src/robosim/vssworld.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "vssworld.h" 20 | #include "vssconfig.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | 28 | bool vssWheelCallBack(dGeomID o1, dGeomID o2, PSurface *surface, int /*robots_count*/) 29 | { 30 | //s->id2 is ground 31 | const dReal *r; //wheels rotation matrix 32 | //const dReal* p; //wheels rotation matrix 33 | if ((o1 == surface->id1) && (o2 == surface->id2)) 34 | { 35 | r = dBodyGetRotation(dGeomGetBody(o1)); 36 | //p=dGeomGetPosition(o1);//never read 37 | } 38 | else if ((o1 == surface->id2) && (o2 == surface->id1)) 39 | { 40 | r = dBodyGetRotation(dGeomGetBody(o2)); 41 | //p=dGeomGetPosition(o2);//never read 42 | } 43 | else 44 | { 45 | //XXX: in this case we dont have the rotation 46 | // matrix, thus we must return 47 | return false; 48 | } 49 | 50 | surface->surface.mode = dContactFDir1 | dContactMu2 | dContactApprox1 | dContactSoftCFM; 51 | surface->surface.mu = fric(VSSConfig::Robot().getWheelPerpendicularFriction()); 52 | surface->surface.mu2 = fric(VSSConfig::Robot().getWheelTangentFriction()); 53 | surface->surface.soft_cfm = 0.002; 54 | 55 | dVector3 v = {0, 0, 1, 1}; 56 | dVector3 axis; 57 | dMultiply0(axis, r, v, 4, 3, 1); 58 | surface->fdir1[0] = axis[0]; 59 | surface->fdir1[1] = axis[1]; 60 | surface->fdir1[2] = 0; 61 | surface->fdir1[3] = 0; 62 | surface->usefdir1 = true; 63 | return true; 64 | } 65 | 66 | VSSWorld::VSSWorld(int fieldType, int nRobotsBlue, int nRobotsYellow, double timeStep, 67 | std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos) 68 | { 69 | this->field.setFieldType(fieldType); 70 | this->field.setRobotsCount(nRobotsBlue + nRobotsYellow); 71 | this->field.setRobotsBlueCount(nRobotsBlue); 72 | this->field.setRobotsYellowCount(nRobotsYellow); 73 | this->stateSize = 5 + nRobotsBlue * 6 + nRobotsYellow * 6; 74 | this->state.reserve(this->stateSize); 75 | this->timeStep = timeStep; 76 | this->physics = new PWorld(this->timeStep, 9.81f, this->field.getRobotsCount()); 77 | this->ball = new PBall(ballPos[0], ballPos[1], VSSConfig::World().getBallRadius(), VSSConfig::World().getBallRadius(), VSSConfig::World().getBallMass()); 78 | this->ground = new PGround(this->field.getFieldRad(), this->field.getFieldLength(), this->field.getFieldWidth(), 79 | this->field.getFieldPenaltyDepth(), this->field.getFieldPenaltyWidth(), this->field.getFieldPenaltyPoint(), 80 | this->field.getFieldLineWidth()); 81 | initWalls(); 82 | 83 | this->physics->addGroundObject(this->ground); 84 | this->physics->addBallObject(this->ball); 85 | for (auto &wall : this->walls) 86 | this->physics->addWallObject(wall); 87 | 88 | for (int k = 0; k < this->field.getRobotsBlueCount(); k++) 89 | { 90 | bool turn_on = true; 91 | std::vector robotPos = blueRobotsPos[k]; 92 | double x = robotPos[0]; 93 | double y = robotPos[1]; 94 | double dir = robotPos[2]; 95 | this->robots[k] = new VSSRobot( 96 | this->physics, this->ball, x, y, VSS_ROBOT_START_Z(), 97 | k + 1, dir, turn_on); 98 | } 99 | for (int k = 0; k < this->field.getRobotsYellowCount(); k++) 100 | { 101 | bool turn_on = true; 102 | std::vector robotPos = yellowRobotsPos[k]; 103 | double x = robotPos[0]; 104 | double y = robotPos[1]; 105 | double dir = robotPos[2]; 106 | this->robots[k + this->field.getRobotsBlueCount()] = new VSSRobot( 107 | this->physics, this->ball, x, y, VSS_ROBOT_START_Z(), 108 | k + 1, dir, turn_on); 109 | } 110 | this->physics->initAllObjects(); 111 | 112 | //Surfaces 113 | PSurface ballwithwall; 114 | ballwithwall.surface.mode = dContactBounce | dContactApprox1; // | dContactSlip1; 115 | ballwithwall.surface.mu = 1; //fric(cfg->ballfriction()); 116 | ballwithwall.surface.bounce = VSSConfig::World().getBallBounce(); 117 | ballwithwall.surface.bounce_vel = VSSConfig::World().getBallBounceVel(); 118 | ballwithwall.surface.slip1 = 0; //cfg->ballslip(); 119 | 120 | PSurface wheelswithground; 121 | PSurface *ball_ground = this->physics->createSurface(this->ball, this->ground); 122 | ball_ground->surface = ballwithwall.surface; 123 | 124 | for (auto &wall : walls) 125 | this->physics->createSurface(this->ball, wall)->surface = ballwithwall.surface; 126 | 127 | for (int k = 0; k < this->field.getRobotsCount(); k++) 128 | { 129 | this->physics->createSurface(robots[k]->chassis, this->ground); 130 | for (auto &wall : walls) 131 | { 132 | this->physics->createSurface(robots[k]->chassis, wall); 133 | } 134 | this->physics->createSurface(robots[k]->chassis, this->ball); 135 | for (auto &wheel : robots[k]->wheels) 136 | { 137 | this->physics->createSurface(wheel->cyl, this->ball); 138 | PSurface *w_g = this->physics->createSurface(wheel->cyl, this->ground); 139 | w_g->surface = wheelswithground.surface; 140 | w_g->usefdir1 = true; 141 | w_g->callback = vssWheelCallBack; 142 | } 143 | for (auto &b : robots[k]->balls) 144 | { 145 | // this->physics->createSurface(b->pBall,this->ball); 146 | PSurface *w_g = this->physics->createSurface(b->pBall, this->ground); 147 | w_g->surface = wheelswithground.surface; 148 | w_g->usefdir1 = true; 149 | w_g->callback = vssWheelCallBack; 150 | } 151 | for (int j = k + 1; j < this->field.getRobotsCount(); j++) 152 | { 153 | if (k != j) 154 | { 155 | this->physics->createSurface(robots[k]->chassis, robots[j]->chassis); //seams ode doesn't understand cylinder-cylinder contacts, so I used spheres 156 | } 157 | } 158 | } 159 | 160 | for (int i = 0; i < 30; i++) 161 | this->physics->step(this->timeStep * 0.1, this->fullSpeed); 162 | replace(ballPos, blueRobotsPos, yellowRobotsPos); 163 | } 164 | 165 | VSSWorld::~VSSWorld() 166 | { 167 | for (auto &wall : this->walls) delete(wall); 168 | for (auto &robot : this->robots) delete(robot); 169 | delete ball; 170 | delete ground; 171 | delete this->physics; 172 | } 173 | 174 | void VSSWorld::initWalls() 175 | { 176 | const double thick = this->field.getWallThickness(); 177 | const double increment = thick / 2; //cfg->Field_Margin() + cfg->Field_Referee_Margin() + thick / 2; 178 | const double pos_x = this->field.getFieldLength() / 2.0 + increment; 179 | const double pos_y = this->field.getFieldWidth() / 2.0 + increment; 180 | const double pos_z = 0.0; 181 | const double siz_x = 2.0 * pos_x; 182 | const double siz_y = 2.0 * pos_y; 183 | const double siz_z = 0.4; 184 | 185 | const double gthick = this->field.getWallThickness(); 186 | const double gpos_x = (this->field.getFieldLength() + gthick) / 2.0 + this->field.getGoalDepth(); 187 | const double gpos_y = (this->field.getGoalWidth() + gthick) / 2.0; 188 | const double gpos_z = 0; //this->field.getGoalHeight() / 2.0; 189 | const double gsiz_x = this->field.getGoalDepth() + gthick; 190 | const double gsiz_y = this->field.getGoalWidth(); 191 | const double gsiz_z = siz_z; //this->field.getGoalHeight(); 192 | const double gpos2_x = (this->field.getFieldLength() + gsiz_x) / 2.0; 193 | 194 | this->walls[0] = new PFixedBox(thick / 2, pos_y, pos_z, 195 | siz_x, thick, siz_z); 196 | 197 | this->walls[1] = new PFixedBox(-thick / 2, -pos_y, pos_z, 198 | siz_x, thick, siz_z); 199 | 200 | this->walls[2] = new PFixedBox(pos_x, gpos_y + (siz_y - gsiz_y) / 4, pos_z, 201 | thick, (siz_y - gsiz_y) / 2, siz_z); 202 | 203 | this->walls[10] = new PFixedBox(pos_x, -gpos_y - (siz_y - gsiz_y) / 4, pos_z, 204 | thick, (siz_y - gsiz_y) / 2, siz_z); 205 | 206 | this->walls[3] = new PFixedBox(-pos_x, gpos_y + (siz_y - gsiz_y) / 4, pos_z, 207 | thick, (siz_y - gsiz_y) / 2, siz_z); 208 | 209 | this->walls[11] = new PFixedBox(-pos_x, -gpos_y - (siz_y - gsiz_y) / 4, pos_z, 210 | thick, (siz_y - gsiz_y) / 2, siz_z); 211 | 212 | // Goal walls 213 | this->walls[4] = new PFixedBox(gpos_x, 0.0, gpos_z, 214 | gthick, gsiz_y, gsiz_z); 215 | 216 | this->walls[5] = new PFixedBox(gpos2_x, -gpos_y, gpos_z, 217 | gsiz_x, gthick, gsiz_z); 218 | 219 | this->walls[6] = new PFixedBox(gpos2_x, gpos_y, gpos_z, 220 | gsiz_x, gthick, gsiz_z); 221 | 222 | this->walls[7] = new PFixedBox(-gpos_x, 0.0, gpos_z, 223 | gthick, gsiz_y, gsiz_z); 224 | 225 | this->walls[8] = new PFixedBox(-gpos2_x, -gpos_y, gpos_z, 226 | gsiz_x, gthick, gsiz_z); 227 | 228 | this->walls[9] = new PFixedBox(-gpos2_x, gpos_y, gpos_z, 229 | gsiz_x, gthick, gsiz_z); 230 | 231 | // Corner Wall 232 | this->walls[12] = new PFixedBox(-pos_x + gsiz_x / 2.8, pos_y - gsiz_x / 2.8, pos_z, 233 | gsiz_x, gthick, gsiz_z); 234 | this->walls[12]->setRotation(0, 0, 1, M_PI / 4); 235 | 236 | this->walls[13] = new PFixedBox(pos_x - gsiz_x / 2.8, pos_y - gsiz_x / 2.8, pos_z, 237 | gsiz_x, gthick, gsiz_z); 238 | this->walls[13]->setRotation(0, 0, 1, -M_PI / 4); 239 | 240 | this->walls[14] = new PFixedBox(pos_x - gsiz_x / 2.8, -pos_y + gsiz_x / 2.8, pos_z, 241 | gsiz_x, gthick, gsiz_z); 242 | this->walls[14]->setRotation(0, 0, 1, M_PI / 4); 243 | 244 | this->walls[15] = new PFixedBox(-pos_x + gsiz_x / 2.8, -pos_y + gsiz_x / 2.8, pos_z, 245 | gsiz_x, gthick, gsiz_z); 246 | this->walls[15]->setRotation(0, 0, 1, -M_PI / 4); 247 | } 248 | 249 | int VSSWorld::robotIndex(unsigned int robot, int team) 250 | { 251 | if (robot >= this->field.getRobotsCount()) 252 | return -1; 253 | return robot + team * this->field.getRobotsBlueCount(); 254 | } 255 | 256 | void VSSWorld::step(std::vector> actions) 257 | { 258 | 259 | setActions(std::move(actions)); 260 | 261 | for (int k = 0; k < this->field.getRobotsCount(); k++) 262 | { 263 | robots[k]->step(); 264 | } 265 | 266 | for (int kk = 0; kk < 5; kk++) 267 | { 268 | const dReal *ballvel = dBodyGetLinearVel(this->ball->body); 269 | // Norma do vetor velocidade da bola 270 | dReal ballSpeed = ballvel[0] * ballvel[0] + ballvel[1] * ballvel[1] + ballvel[2] * ballvel[2]; 271 | ballSpeed = sqrt(ballSpeed); 272 | if (ballSpeed > 0.01) { 273 | dReal fk = VSSConfig::World().getBallFriction() * VSSConfig::World().getBallMass() * VSSConfig::World().getGravity(); 274 | dReal ballfx = -fk * ballvel[0] / ballSpeed; 275 | dReal ballfy = -fk * ballvel[1] / ballSpeed; 276 | dReal ballfz = -fk * ballvel[2] / ballSpeed; 277 | dReal balltx = -ballfy * VSSConfig::World().getBallRadius(); 278 | dReal ballty = ballfx * VSSConfig::World().getBallRadius(); 279 | dReal balltz = 0; 280 | dBodyAddTorque(this->ball->body,balltx,ballty,balltz); 281 | dBodyAddForce(this->ball->body,ballfx,ballfy,ballfz); 282 | } else { 283 | dBodySetAngularVel(this->ball->body, 0, 0, 0); 284 | dBodySetLinearVel(this->ball->body, 0, 0, 0); 285 | } 286 | 287 | this->physics->step(this->timeStep * 0.2, this->fullSpeed); 288 | } 289 | 290 | } 291 | 292 | void VSSWorld::setActions(std::vector> actions) 293 | { 294 | for (int i = 0; i < this->field.getRobotsCount(); i++) 295 | { 296 | std::vector rbtAction = actions[i]; 297 | robots[i]->setWheelDesiredAngularSpeed(0, -1 * rbtAction[0]); 298 | robots[i]->setWheelDesiredAngularSpeed(1, rbtAction[1]); 299 | } 300 | } 301 | 302 | const std::unordered_map VSSWorld::getFieldParams() 303 | { 304 | std::unordered_map fieldParams; 305 | 306 | fieldParams["length"] = this->field.getFieldLength(); 307 | fieldParams["width"] = this->field.getFieldWidth(); 308 | fieldParams["penalty_length"] = this->field.getFieldPenaltyDepth(); 309 | fieldParams["penalty_width"] = this->field.getFieldPenaltyWidth(); 310 | fieldParams["goal_width"] = this->field.getGoalWidth(); 311 | fieldParams["goal_depth"] = this->field.getGoalDepth(); 312 | fieldParams["ball_radius"] = VSSConfig::World().getBallRadius(); 313 | fieldParams["rbt_distance_center_kicker"] = -1.; 314 | fieldParams["rbt_kicker_thickness"] = -1.; 315 | fieldParams["rbt_kicker_width"] = -1.; 316 | fieldParams["rbt_wheel0_angle"] = VSSConfig::Robot().getWheel0Angle(); 317 | fieldParams["rbt_wheel1_angle"] = VSSConfig::Robot().getWheel1Angle(); 318 | fieldParams["rbt_wheel2_angle"] = -1.; 319 | fieldParams["rbt_wheel3_angle"] = -1.; 320 | fieldParams["rbt_radius"] = VSSConfig::Robot().getRadius(); 321 | fieldParams["rbt_wheel_radius"] = VSSConfig::Robot().getWheelRadius(); 322 | fieldParams["rbt_motor_max_rpm"] = VSSConfig::Robot().getWheelMotorMaxRPM(); 323 | return fieldParams; 324 | } 325 | 326 | const std::vector &VSSWorld::getState() 327 | { 328 | std::vector lastState; 329 | lastState = this->state; 330 | this->state.clear(); 331 | 332 | dReal ballX, ballY, ballZ; 333 | dReal robotX, robotY, robotDir, robotK; 334 | const dReal *ballVel, *robotVel, *robotVelDir; 335 | 336 | // Ball 337 | this->ball->getBodyPosition(ballX, ballY, ballZ); 338 | 339 | // Add ball position to state vector 340 | this->state.push_back(ballX); 341 | this->state.push_back(ballY); 342 | this->state.push_back(ballZ); 343 | if (lastState.size() > 0) 344 | { 345 | this->state.push_back((ballX - lastState[0]) / this->timeStep); 346 | this->state.push_back((ballY - lastState[1]) / this->timeStep); 347 | } 348 | else 349 | { 350 | ballVel = dBodyGetLinearVel(this->ball->body); 351 | this->state.push_back(0.); 352 | this->state.push_back(0.); 353 | } 354 | 355 | // Robots 356 | for (uint32_t i = 0; i < this->field.getRobotsCount(); i++) 357 | { 358 | this->robots[i]->getXY(robotX, robotY); 359 | 360 | // robotDir is not currently being used 361 | robotDir = this->robots[i]->getDir(robotK); 362 | robotVel = dBodyGetLinearVel(this->robots[i]->chassis->body); 363 | robotVelDir = dBodyGetAngularVel(this->robots[i]->chassis->body); 364 | // reset when the robot has turned over 365 | if (VSSConfig::World().getResetTurnOver() && robotK < 0.9) 366 | { 367 | std::cout << "turnover " << robotK << '\n'; 368 | this->robots[i]->resetRobot(); 369 | } 370 | 371 | // Add robot position to state vector 372 | this->state.push_back(robotX); 373 | this->state.push_back(robotY); 374 | this->state.push_back(robotDir); 375 | if (lastState.size() > 0) 376 | { 377 | this->state.push_back((robotX - lastState[5 + (6 * i) + 0]) / this->timeStep); 378 | this->state.push_back((robotY - lastState[5 + (6 * i) + 1]) / this->timeStep); 379 | this->state.push_back(smallestAngleDiff(robotDir, lastState[5 + (6 * i) + 2]) / this->timeStep); 380 | } 381 | else 382 | { 383 | this->state.push_back(0.); 384 | this->state.push_back(0.); 385 | this->state.push_back(0.); 386 | } 387 | } 388 | 389 | return this->state; 390 | } 391 | 392 | void VSSWorld::replace(std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos) 393 | { 394 | dReal xx, yy, zz; 395 | this->ball->getBodyPosition(xx, yy, zz); 396 | this->ball->setBodyPosition(ballPos[0], ballPos[1], zz); 397 | dBodySetLinearVel(this->ball->body, ballPos[2], ballPos[3], 0); 398 | dBodySetAngularVel(this->ball->body, 0, 0, 0); 399 | 400 | for (uint32_t i = 0; i < this->field.getRobotsBlueCount(); i++) 401 | { 402 | std::vector robotPos = blueRobotsPos[i]; 403 | this->robots[i]->resetRobot(); 404 | this->robots[i]->setXY(robotPos[0], robotPos[1]); 405 | this->robots[i]->setDir(robotPos[2]); 406 | } 407 | 408 | for (int32_t i = 0; i < this->field.getRobotsYellowCount(); i++) 409 | { 410 | int k = i + this->field.getRobotsBlueCount(); 411 | std::vector robotPos = yellowRobotsPos[i]; 412 | this->robots[k]->resetRobot(); 413 | this->robots[k]->setXY(robotPos[0], robotPos[1]); 414 | this->robots[k]->setDir(robotPos[2]); 415 | } 416 | 417 | } 418 | -------------------------------------------------------------------------------- /src/robosim/sslworld.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | grSim - RoboCup Small Size Soccer Robots Simulator 3 | Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim) 4 | 5 | This program is free software: you can redistribute it and/or modify 6 | it under the terms of the GNU General Public License as published by 7 | the Free Software Foundation, either version 3 of the License, or 8 | (at your option) any later version. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | */ 18 | 19 | #include "sslworld.h" 20 | #include "sslconfig.h" 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | bool sslWheelCallBack(dGeomID o1, dGeomID o2, PSurface *surface, int /*robots_count*/) 28 | { 29 | //s->id2 is ground 30 | const dReal *r; //wheels rotation matrix 31 | //const dReal* p; //wheels rotation matrix 32 | if ((o1 == surface->id1) && (o2 == surface->id2)) 33 | { 34 | r = dBodyGetRotation(dGeomGetBody(o1)); 35 | //p=dGeomGetPosition(o1);//never read 36 | } 37 | else if ((o1 == surface->id2) && (o2 == surface->id1)) 38 | { 39 | r = dBodyGetRotation(dGeomGetBody(o2)); 40 | //p=dGeomGetPosition(o2);//never read 41 | } 42 | else 43 | { 44 | //XXX: in this case we dont have the rotation 45 | // matrix, thus we must return 46 | return false; 47 | } 48 | 49 | surface->surface.mode = dContactFDir1 | dContactMu2 | dContactApprox1 | dContactSoftCFM; 50 | surface->surface.mu = fric(SSLConfig::Robot().getWheelPerpendicularFriction()); 51 | surface->surface.mu2 = fric(SSLConfig::Robot().getWheelTangentFriction()); 52 | surface->surface.soft_cfm = 0.002; 53 | 54 | dVector3 v = {0, 0, 1, 1}; 55 | dVector3 axis; 56 | dMultiply0(axis, r, v, 4, 3, 1); 57 | // dReal l = sqrt(axis[0] * axis[0] + axis[1] * axis[1]); 58 | surface->fdir1[0] = axis[0]; 59 | surface->fdir1[1] = axis[1]; 60 | surface->fdir1[2] = 0; 61 | surface->fdir1[3] = 0; 62 | surface->usefdir1 = true; 63 | return true; 64 | } 65 | 66 | bool sslBallCallBack(dGeomID o1, dGeomID o2, PSurface *surface, int /*robots_count*/) 67 | { 68 | auto body = dGeomGetBody(o2); 69 | const dReal *posBall = dBodyGetPosition(body); 70 | body = dGeomGetBody(o1); 71 | const dReal *posRobot = dBodyGetPosition(body); 72 | const dReal *dirRobot = dBodyGetRotation(body); 73 | 74 | dReal distRbtBall = sqrt(pow(posRobot[0]-posBall[0],2) + pow(posRobot[1]-posBall[1],2)); 75 | if (distRbtBall < SSLConfig::Robot().getDistanceCenterKicker() + SSLConfig::World().getBallRadius()/2.) return true; 76 | 77 | // Get robot angle 78 | dVector3 v={1,0,0}; 79 | dVector3 axis; 80 | dMultiply0(axis,dirRobot,v,4,3,1); 81 | dReal dot = axis[0]; 82 | dReal length = sqrt(axis[0]*axis[0] + axis[1]*axis[1]); 83 | dReal absAng = (dReal)(acos((dReal)(dot/length))); 84 | dReal angleRobot = (axis[1] > 0) ? absAng : -absAng; 85 | 86 | // Get angle between robot and ball 87 | dReal angleRobotBall = atan2(posBall[1] - posRobot[1],posBall[0]-posRobot[0]); 88 | 89 | // This value is given by the acos(distance_center_kicker/robot_radius) 90 | dReal angleKicker = SSLConfig::Robot().getKickerAngle(); 91 | 92 | // Smallest angle diff 93 | dReal angleDiff = angleRobotBall - angleRobot; 94 | angleDiff += (angleDiff>M_PI) ? -2*M_PI : (angleDiff<-M_PI) ? 2*M_PI : 0; 95 | 96 | // If kicker is facing the ball, the collision with the chassis should not 97 | // be considered 98 | return (abs(angleDiff) < angleKicker) ? false : true; 99 | } 100 | 101 | SSLWorld::SSLWorld(int fieldType, int nRobotsBlue, int nRobotsYellow, double timeStep, 102 | std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos) 103 | { 104 | // fieldType = 0 for Div A, fieldType = 1 for Div B 105 | this->field.setFieldType(fieldType); 106 | this->field.setRobotsCount(nRobotsBlue + nRobotsYellow); 107 | this->field.setRobotsBlueCount(nRobotsBlue); 108 | this->field.setRobotsYellowCount(nRobotsYellow); 109 | this->stateSize = 5 + nRobotsBlue * 7 + nRobotsYellow * 7; 110 | this->state.reserve(this->stateSize); 111 | this->timeStep = timeStep; 112 | this->physics = new PWorld(this->timeStep, 9.81f, this->field.getRobotsCount()); 113 | this->ball = new PBall(ballPos[0], ballPos[1], SSLConfig::World().getBallRadius(), SSLConfig::World().getBallRadius(), SSLConfig::World().getBallMass()); 114 | this->ground = new PGround(this->field.getFieldRad(), this->field.getFieldLength(), this->field.getFieldWidth(), 115 | this->field.getFieldPenaltyDepth(), this->field.getFieldPenaltyWidth(), this->field.getFieldPenaltyPoint(), 116 | this->field.getFieldLineWidth()); 117 | 118 | initWalls(); 119 | 120 | this->physics->addGroundObject(this->ground); 121 | this->physics->addBallObject(this->ball); 122 | 123 | for (auto &wall : this->walls) 124 | this->physics->addWallObject(wall); 125 | 126 | for (int k = 0; k < this->field.getRobotsBlueCount(); k++) 127 | { 128 | bool turn_on = true; 129 | std::vector robotPos = blueRobotsPos[k]; 130 | double x = robotPos[0]; 131 | double y = robotPos[1]; 132 | double dir = robotPos[2]; 133 | this->robots[k] = new SSLRobot( 134 | this->physics, this->ball, x, y, SSL_ROBOT_START_Z(), 135 | k + 1, dir); 136 | } 137 | for (int k = 0; k < this->field.getRobotsYellowCount(); k++) 138 | { 139 | bool turn_on = true; 140 | std::vector robotPos = yellowRobotsPos[k]; 141 | double x = robotPos[0]; 142 | double y = robotPos[1]; 143 | double dir = robotPos[2]; 144 | robots[k + this->field.getRobotsBlueCount()] = new SSLRobot( 145 | this->physics, this->ball, x, y, SSL_ROBOT_START_Z(), 146 | k + 1, dir); 147 | } 148 | 149 | this->physics->initAllObjects(); 150 | 151 | //Surfaces 152 | PSurface ballwithwall; 153 | ballwithwall.surface.mode = dContactBounce | dContactApprox1; // | dContactSlip1; 154 | ballwithwall.surface.mu = 1; //fric(cfg->ballfriction()); 155 | ballwithwall.surface.bounce = SSLConfig::World().getBallBounce(); 156 | ballwithwall.surface.bounce_vel = SSLConfig::World().getBallBounceVel(); 157 | ballwithwall.surface.slip1 = 0; //cfg->ballslip(); 158 | 159 | PSurface wheelswithground; 160 | PSurface *ball_ground = this->physics->createOneWaySurface(this->ball, this->ground); 161 | ball_ground->surface = ballwithwall.surface; 162 | 163 | PSurface ballwithkicker; 164 | ballwithkicker.surface.mode = dContactApprox1; 165 | ballwithkicker.surface.mu = fric(SSLConfig::Robot().getKickerFriction()); 166 | ballwithkicker.surface.slip1 = 5; 167 | 168 | for (auto &wall : walls) 169 | this->physics->createOneWaySurface(this->ball, wall)->surface = ballwithwall.surface; 170 | 171 | for (int k = 0; k < this->field.getRobotsCount(); k++) 172 | { 173 | for (auto &wall : walls) 174 | { 175 | this->physics->createOneWaySurface(this->robots[k]->chassis, wall); 176 | } 177 | 178 | // Create surface between ball and chassis 179 | PSurface* ballChassis = this->physics->createOneWaySurface(this->robots[k]->chassis, this->ball); 180 | ballChassis->callback = sslBallCallBack; 181 | 182 | this->physics->createOneWaySurface(this->ball, this->robots[k]->kicker->box)->surface = ballwithkicker.surface; 183 | for (auto &wheel : this->robots[k]->wheels) 184 | { 185 | PSurface *w_g = this->physics->createOneWaySurface(wheel->cyl, this->ground); 186 | w_g->surface = wheelswithground.surface; 187 | w_g->usefdir1 = true; 188 | w_g->callback = sslWheelCallBack; 189 | } 190 | for (int j = k + 1; j < this->field.getRobotsCount(); j++) 191 | { 192 | if (k != j) 193 | { 194 | this->physics->createSurface(this->robots[k]->chassis, this->robots[j]->chassis); 195 | } 196 | } 197 | } 198 | 199 | for (int i = 0; i < 30; i++) 200 | this->physics->step(this->timeStep * 0.1, this->fullSpeed); 201 | replace(ballPos, blueRobotsPos, yellowRobotsPos); 202 | } 203 | 204 | SSLWorld::~SSLWorld() 205 | { 206 | for (auto &wall : this->walls) delete(wall); 207 | for (auto &robot : this->robots) delete(robot); 208 | delete ball; 209 | delete ground; 210 | delete this->physics; 211 | } 212 | 213 | void SSLWorld::initWalls() 214 | { 215 | const double thick = this->field.getWallThickness(); 216 | const double increment = this->field.getFieldMargin() + this->field.getFieldRefereeMargin() + thick / 2; 217 | const double pos_x = this->field.getFieldLength() / 2.0 + increment; 218 | const double pos_y = this->field.getFieldWidth() / 2.0 + increment; 219 | const double pos_z = 0.0; 220 | const double siz_x = 2.0 * pos_x; 221 | const double siz_y = 2.0 * pos_y; 222 | const double siz_z = 0.4; 223 | 224 | const double gthick = this->field.getGoalThickness(); 225 | const double gpos_x = (this->field.getFieldLength() + gthick) / 2.0 + this->field.getGoalDepth(); 226 | const double gpos_y = (this->field.getGoalWidth() + gthick) / 2.0; 227 | const double gpos_z = this->field.getGoalHeight() / 2.0; 228 | const double gsiz_x = this->field.getGoalDepth() + gthick; 229 | const double gsiz_y = this->field.getGoalWidth(); 230 | const double gsiz_z = this->field.getGoalHeight(); 231 | const double gpos2_x = (this->field.getFieldLength() + gsiz_x) / 2.0; 232 | 233 | this->walls[0] = new PFixedBox(thick/2, pos_y, pos_z, 234 | siz_x, thick, siz_z); 235 | 236 | this->walls[1] = new PFixedBox(-thick/2, -pos_y, pos_z, 237 | siz_x, thick, siz_z); 238 | 239 | this->walls[2] = new PFixedBox(pos_x, -thick/2, pos_z, 240 | thick, siz_y, siz_z); 241 | 242 | this->walls[3] = new PFixedBox(-pos_x, thick/2, pos_z, 243 | thick, siz_y, siz_z); 244 | 245 | // Goal walls 246 | 247 | this->walls[4] = new PFixedBox(gpos_x, 0.0, gpos_z, 248 | gthick, gsiz_y, gsiz_z); 249 | 250 | this->walls[5] = new PFixedBox(gpos2_x, -gpos_y, gpos_z, 251 | gsiz_x, gthick, gsiz_z); 252 | 253 | this->walls[6] = new PFixedBox(gpos2_x, gpos_y, gpos_z, 254 | gsiz_x, gthick, gsiz_z); 255 | 256 | this->walls[7] = new PFixedBox(-gpos_x, 0.0, gpos_z, 257 | gthick, gsiz_y, gsiz_z); 258 | 259 | this->walls[8] = new PFixedBox(-gpos2_x, -gpos_y, gpos_z, 260 | gsiz_x, gthick, gsiz_z); 261 | 262 | this->walls[9] = new PFixedBox(-gpos2_x, gpos_y, gpos_z, 263 | gsiz_x, gthick, gsiz_z); 264 | } 265 | 266 | 267 | void SSLWorld::step(std::vector> actions) 268 | { 269 | 270 | setActions(std::move(actions)); 271 | 272 | for (int k = 0; k < this->field.getRobotsCount(); k++) 273 | { 274 | robots[k]->step(); 275 | } 276 | 277 | // Pq ele faz isso 5 vezes? 278 | // - Talvez mais precisao (Ele sempre faz um step de dt*0.2 ) 279 | for (int kk = 0; kk < 5; kk++) 280 | { 281 | const dReal *ballvel = dBodyGetLinearVel(this->ball->body); 282 | // Norma do vetor velocidade da bola 283 | dReal ballSpeed = ballvel[0] * ballvel[0] + ballvel[1] * ballvel[1] + ballvel[2] * ballvel[2]; 284 | ballSpeed = sqrt(ballSpeed); 285 | if (ballSpeed > 0.01) { 286 | dReal fk = SSLConfig::World().getBallFriction() * SSLConfig::World().getBallMass() * SSLConfig::World().getGravity(); 287 | dReal ballfx = -fk * ballvel[0] / ballSpeed; 288 | dReal ballfy = -fk * ballvel[1] / ballSpeed; 289 | dReal ballfz = -fk * ballvel[2] / ballSpeed; 290 | dReal balltx = -ballfy * SSLConfig::World().getBallRadius(); 291 | dReal ballty = ballfx * SSLConfig::World().getBallRadius(); 292 | dReal balltz = 0; 293 | dBodyAddTorque(this->ball->body,balltx,ballty,balltz); 294 | dBodyAddForce(this->ball->body,ballfx,ballfy,ballfz); 295 | } else { 296 | dBodySetAngularVel(this->ball->body, 0, 0, 0); 297 | dBodySetLinearVel(this->ball->body, 0, 0, 0); 298 | } 299 | 300 | this->physics->step(this->timeStep * 0.2, this->fullSpeed); 301 | } 302 | 303 | } 304 | 305 | void SSLWorld::setActions(std::vector> actions) 306 | { 307 | for (int i = 0; i < this->field.getRobotsCount(); i++) 308 | { 309 | std::vector rbtAction = actions[i]; 310 | if (rbtAction[0] > 0) { 311 | this->robots[i]->setWheelDesiredAngularSpeed(0, rbtAction[1]); 312 | this->robots[i]->setWheelDesiredAngularSpeed(1, rbtAction[2]); 313 | this->robots[i]->setWheelDesiredAngularSpeed(2, rbtAction[3]); 314 | this->robots[i]->setWheelDesiredAngularSpeed(3, rbtAction[4]); 315 | } 316 | else this->robots[i]->setDesiredSpeedLocal(rbtAction[1], rbtAction[2], rbtAction[3]); 317 | if (rbtAction[5] > 0 || rbtAction[6] > 0) { 318 | this->robots[i]->kicker->kick(rbtAction[5], rbtAction[6]); 319 | } 320 | if (rbtAction[7] > 0) this->robots[i]->kicker->setDribbler(true); 321 | } 322 | } 323 | 324 | const std::unordered_map SSLWorld::getFieldParams() 325 | { 326 | std::unordered_map fieldParams; 327 | 328 | fieldParams["length"] = this->field.getFieldLength(); 329 | fieldParams["width"] = this->field.getFieldWidth(); 330 | fieldParams["penalty_length"] = this->field.getFieldPenaltyDepth(); 331 | fieldParams["penalty_width"] = this->field.getFieldPenaltyWidth(); 332 | fieldParams["goal_width"] = this->field.getGoalWidth(); 333 | fieldParams["goal_depth"] = this->field.getGoalDepth(); 334 | fieldParams["ball_radius"] = SSLConfig::World().getBallRadius(); 335 | fieldParams["rbt_distance_center_kicker"] = SSLConfig::Robot().getDistanceCenterKicker(); 336 | fieldParams["rbt_kicker_thickness"] = SSLConfig::Robot().getKickerThickness(); 337 | fieldParams["rbt_kicker_width"] = SSLConfig::Robot().getKickerWidth(); 338 | fieldParams["rbt_wheel0_angle"] = SSLConfig::Robot().getWheel0Angle(); 339 | fieldParams["rbt_wheel1_angle"] = SSLConfig::Robot().getWheel1Angle(); 340 | fieldParams["rbt_wheel2_angle"] = SSLConfig::Robot().getWheel2Angle(); 341 | fieldParams["rbt_wheel3_angle"] = SSLConfig::Robot().getWheel3Angle(); 342 | fieldParams["rbt_radius"] = SSLConfig::Robot().getRadius(); 343 | fieldParams["rbt_wheel_radius"] = SSLConfig::Robot().getWheelRadius(); 344 | fieldParams["rbt_motor_max_rpm"] = SSLConfig::Robot().getWheelMotorMaxRPM(); 345 | 346 | return fieldParams; 347 | } 348 | 349 | const std::vector &SSLWorld::getState() 350 | { 351 | std::vector lastState; 352 | lastState = this->state; 353 | this->state.clear(); 354 | 355 | dReal ballX, ballY, ballZ; 356 | dReal robotX, robotY, robotDir, robotK; 357 | const dReal *ballVel, *robotVel, *robotVelDir; 358 | 359 | // Ball 360 | this->ball->getBodyPosition(ballX, ballY, ballZ); 361 | 362 | // Add ball position to state vector 363 | this->state.push_back(ballX); 364 | this->state.push_back(ballY); 365 | this->state.push_back(ballZ); 366 | if (lastState.size() > 0) 367 | { 368 | this->state.push_back((ballX - lastState[0]) / this->timeStep); 369 | this->state.push_back((ballY - lastState[1]) / this->timeStep); 370 | } 371 | else 372 | { 373 | ballVel = dBodyGetLinearVel(this->ball->body); 374 | this->state.push_back(0.); 375 | this->state.push_back(0.); 376 | } 377 | 378 | // Robots 379 | for (uint32_t i = 0; i < this->field.getRobotsCount(); i++) 380 | { 381 | this->robots[i]->getXY(robotX, robotY); 382 | 383 | // robotDir is not currently being used 384 | robotDir = this->robots[i]->getDir(robotK); 385 | robotVel = dBodyGetLinearVel(this->robots[i]->chassis->body); 386 | robotVelDir = dBodyGetAngularVel(this->robots[i]->chassis->body); 387 | // reset when the robot has turned over 388 | if (SSLConfig::World().getResetTurnOver() && robotK < 0.9) 389 | { 390 | std::cout << "turnover " << robotK << " robot x: " << robotX << " robot y: " << robotY << " robot dir: " << robotDir << " ball x: " << ballX << " ball y: " << ballY << '\n'; 391 | this->robots[i]->resetRobot(); 392 | } 393 | 394 | // Add robot position to state vector 395 | this->state.push_back(robotX); 396 | this->state.push_back(robotY); 397 | this->state.push_back(robotDir); 398 | if (lastState.size() > 0) 399 | { 400 | this->state.push_back((robotX - lastState[5 + (11 * i) + 0]) / this->timeStep); 401 | this->state.push_back((robotY - lastState[5 + (11 * i) + 1]) / this->timeStep); 402 | this->state.push_back(smallestAngleDiff(robotDir, lastState[5 + (11 * i) + 2]) / this->timeStep); 403 | } 404 | else 405 | { 406 | this->state.push_back(0.); 407 | this->state.push_back(0.); 408 | this->state.push_back(0.); 409 | } 410 | this->state.push_back(static_cast(this->robots[i]->kicker->isTouchingBall())); 411 | 412 | // Get wheel speeds 413 | this->state.push_back(this->robots[i]->wheels[0]->desiredAngularSpeed); 414 | this->state.push_back(this->robots[i]->wheels[1]->desiredAngularSpeed); 415 | this->state.push_back(this->robots[i]->wheels[2]->desiredAngularSpeed); 416 | this->state.push_back(this->robots[i]->wheels[3]->desiredAngularSpeed); 417 | } 418 | 419 | return this->state; 420 | } 421 | 422 | void SSLWorld::replace(std::vector ballPos, std::vector> blueRobotsPos, std::vector> yellowRobotsPos) 423 | { 424 | dReal xx, yy, zz; 425 | this->ball->getBodyPosition(xx, yy, zz); 426 | this->ball->setBodyPosition(ballPos[0], ballPos[1], zz); 427 | dBodySetLinearVel(this->ball->body, ballPos[2], ballPos[3], 0); 428 | dBodySetAngularVel(this->ball->body, 0, 0, 0); 429 | 430 | for (uint32_t i = 0; i < this->field.getRobotsBlueCount(); i++) 431 | { 432 | std::vector robotPos = blueRobotsPos[i]; 433 | this->robots[i]->resetRobot(); 434 | this->robots[i]->setXY(robotPos[0], robotPos[1]); 435 | this->robots[i]->setDir(robotPos[2]); 436 | } 437 | 438 | for (int32_t i = 0; i < this->field.getRobotsYellowCount(); i++) 439 | { 440 | int k = i + this->field.getRobotsBlueCount(); 441 | std::vector robotPos = yellowRobotsPos[i]; 442 | this->robots[k]->resetRobot(); 443 | this->robots[k]->setXY(robotPos[0], robotPos[1]); 444 | this->robots[k]->setDir(robotPos[2]); 445 | } 446 | } 447 | --------------------------------------------------------------------------------