diff --git a/pyrobolearn/simulators/raisim.py b/pyrobolearn/simulators/raisim.py index 90202ef..f0b5c98 100644 --- a/pyrobolearn/simulators/raisim.py +++ b/pyrobolearn/simulators/raisim.py @@ -1,34 +1,43 @@ #!/usr/bin/env python """Define the RaiSim Simulator API. -Warnings: Currently, the RaiSim simulator is closed-source and is only available for researchers at RSL and ETH Zurich. +This is the main interface that communicates with the RaiSim simulator [1-5]. By defining this interface, it +allows to decouple the PyRoboLearn framework from the simulator. It also converts some data types to the ones required +by RaiSim. Because it didn't have a Python wrapper, one has been written in the ``raisim_wrapper`` folder using +pybind11 [6]. -This is the main interface that communicates with the RaiSim simulator [1, 2]. By defining this interface, it allows to -decouple the PyRoboLearn framework from the simulator. It also converts some data types to the ones required by -RaiSim. +The signature of each method defined here are inspired by [1,2] but in accordance with the PEP8 style guide [7]. +Parts of the documentation for the methods have been copied-pasted from [2-5] for completeness purposes. -The signature of each method defined here are inspired by [1,2] but in accordance with the PEP8 style guide [3]. -Parts of the documentation for the methods have been copied-pasted from [2] for completeness purposes. +RaiSim is distributed under the End-User License Agreement (EULA) [8], and officially works on Ubuntu 16.04 and 18.04. Dependencies in PRL: * `pyrobolearn.simulators.simulator.Simulator` References: - [1] "Per-Contact Iteration Method for Solving Contact Dynamics", Hwangbo et al., 2018 - [2] RaiSim: https://leggedrobotics.github.io/SimBenchmark/about/sims.html - [3] PEP8: https://www.python.org/dev/peps/pep-0008/ + - [1] "Per-Contact Iteration Method for Solving Contact Dynamics", Hwangbo et al., 2018 + - [2] RaiSim benchmarks: https://leggedrobotics.github.io/SimBenchmark/about/sims.html + - [3] RaiSim, a physics engine for robotics and AI research: https://github.com/leggedrobotics/raisimLib + - [4] raisimOgre - Visualizer for raisim: https://github.com/leggedrobotics/raisimOgre + - [5] raisimGym - RL examples using raisim: https://github.com/leggedrobotics/raisimGym + - [6] pybind11 (documentation): https://pybind11.readthedocs.io/en/stable/ + - [7] PEP8: https://www.python.org/dev/peps/pep-0008/ + - [8] RaiSim license: https://github.com/leggedrobotics/raisimLib/blob/master/LICENSE.md """ -# TODO: -# 1. wait for ETH to release the simulator (not sure if they will ever do it...) -# 2. check if a Python wrapper is provided, if not, will have to implement it +# import raisim +try: + import raisimpy as raisim +except ImportError as e: + print(e.__str__() + "\nHINT: you need to install `raisimLib` and `raisimOgre`, and build the Python wrappers " + "that are located in the `raisim_wrapper` folder.") # import PRL simulator from pyrobolearn.simulators.simulator import Simulator __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["RaiSim (ETHz)", "Brian Delhaisse"] +__credits__ = ["RaiSim (ETHz, Hwangbo, Kang, Lee)", "Brian Delhaisse (Python wrappers + PRL)"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -52,6 +61,14 @@ class Raisim(Simulator): def __init__(self, render=True, **kwargs): super(Raisim, self).__init__(render, **kwargs) + + # create world + self.world = raisim.World() + self.sim = self.world # alias + + # create visualizer if specified + self.visualizer = None + raise NotImplementedError("The RaiSim simulator is not currently available as it has not been released for " "the moment") diff --git a/pyrobolearn/simulators/raisim_wrapper/CMakeLists.txt b/pyrobolearn/simulators/raisim_wrapper/CMakeLists.txt new file mode 100644 index 0000000..4353e17 --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/CMakeLists.txt @@ -0,0 +1,25 @@ +# Run: cmake -DPYBIND11_PYTHON_VERSION= -DCMAKE_PREFIX_PATH=$LOCAL_BUILD .. +# where =2.7 or 3.*, LOCAL_BUILD is the build for raisimLib and raisimOgre + +cmake_minimum_required(VERSION 2.8.9) +project (raisim_wrapper) + +# find the various packages +find_package(pybind11 REQUIRED) +find_package(Eigen3 REQUIRED eigen3) +# find_package(OpenMP REQUIRED) +find_package(raisim CONFIG REQUIRED) +find_package(raisimOgre CONFIG REQUIRED) + +# header files +include_directories(include ${EIGEN3_INCLUDE_DIRS}) + +# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + +# source files +file(GLOB SOURCES "src/*.cpp") + +pybind11_add_module(raisimpy ${SOURCES}) +target_link_libraries(raisimpy raisim::raisim raisim::raisimOgre) diff --git a/pyrobolearn/simulators/raisim_wrapper/LICENSE b/pyrobolearn/simulators/raisim_wrapper/LICENSE new file mode 100644 index 0000000..6a2c937 --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019, Brian Delhaisse + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/pyrobolearn/simulators/raisim_wrapper/README.rst b/pyrobolearn/simulators/raisim_wrapper/README.rst new file mode 100644 index 0000000..5b2c845 --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/README.rst @@ -0,0 +1,61 @@ +Python wrapper for RaiSim +========================= + +This folder contains a python wrapper around RaiSim (``raisimLib`` and ``raisimOgre``) using ``pybind11``. + +Parts of the wrappers were taken and modified from (or inspired by) the code given in the ``raisimGym/raisim_gym/env/`` +folder. If you use these wrappers in PRL, please acknowledge their contribution as well by citing [1-4]. + + +How to use the wrappers? +~~~~~~~~~~~~~~~~~~~~~~~~ + +In order to use the wrappers, you will have to install at least +`raisimLib `_ and +`raisimOgre `_. You will also have to install +`pybind11 `_ as we use this to wrap the C++ code. + +Then you will have to compile the code from the ``raisim_wrapper`` folder by typing: + +.. code-block:: bash + + mkdir build && cd build + cmake -DPYBIND11_PYTHON_VERSION=$PYTHON_VERSION -DCMAKE_PREFIX_PATH=$LOCAL_BUILD .. + make + +where ``PYTHON_VERSION=2.7 or 3.*`` and ``LOCAL_BUILD`` is the build directory where we installed the exported cmake +libraries (as described in [2-4]). + +Once it has been compiled, you can access to the Python library ``raisim`` in your code with: + +.. code-block:: python + + import raisim + + print(dir(raisim)) + + +References +~~~~~~~~~~ + +- [1] "Per-contact iteration method for solving contact dynamics", Hwangbo et al., 2018 +- [2] raisimLib: https://github.com/leggedrobotics/raisimLib +- [3] raisimOgre: https://github.com/leggedrobotics/raisimOgre +- [4] raisimGym: https://github.com/leggedrobotics/raisimGym +- [5] pybind11: https://pybind11.readthedocs.io/en/stable/ + + +Troubleshooting +~~~~~~~~~~~~~~~ + +- ``fatal error: Eigen/*: No such file or directory`` + - If you have Eigen3 installed on your system, you probably have to replace all the ``#include `` by + ``#include ``. You can create symlinks to solve this issue: + + .. code-block:: bash + + cd /usr/local/include + sudo ln -sf eigen3/Eigen Eigen + sudo ln -sf eigen3/unsupported unsupported + + or you can replace the ``#include `` by ``#include ``. \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/include/converter.hpp b/pyrobolearn/simulators/raisim_wrapper/include/converter.hpp new file mode 100644 index 0000000..a4355cb --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/include/converter.hpp @@ -0,0 +1,141 @@ +/** + * Type converters used to convert between different data types. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#ifndef CONVERTER_H +#define CONVERTER_H + +#include +#include // numpy types + +#include // for ostringstream +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, VecDyn, MatDyn, etc. + +namespace py = pybind11; + + +/// \brief: convert from raisim::Vec to np.array +template +py::array_t convert_vec_to_np(const raisim::Vec &vec) { + const double *ptr = vec.ptr(); // get data pointer + + // return np.array[float64[n]] + return py::array_t( + {n}, // shape + {sizeof(double)}, // C-style contiguous strides for double (double=8 bytes) + ptr); // data pointer +// vec); // numpy array references this parent +} + + +/// \brief: convert from np.array[float[n]] to raisim::Vec +template +raisim::Vec convert_np_to_vec(py::array_t array) { + + // check size + if (array.size() != n) { + std::ostringstream s; + s << "error: expecting the given vector to be of size " << n << " but got instead a size of " + << array.size() << "."; + throw std::domain_error(s.str()); + } + + // reshape if necessary + if (array.ndim() > 1) + array.resize({n}); + + // create raisim vector + raisim::Vec vec; + + // copy the data + for(size_t i=0; i to np.array[float64[n,m]] +template +py::array_t convert_mat_to_np(const raisim::Mat &mat) { + const double *ptr = mat.ptr(); // get data pointer + + // return np.array[float64[n,m]] + return py::array_t( + {n, m}, // shape + {sizeof(double), sizeof(double)}, // C-style contiguous strides for double (double=8bytes) + ptr); +// mat); // numpy array references this parent +} + + +/// \brief: convert from np.array[float[n,m]] to raisim::Mat +template +raisim::Mat convert_np_to_mat(py::array_t array) { + + // check dimensions and shape + if (array.ndim() != 2) { + std::ostringstream s; + s << "error: expecting the given array to have a dimension of 2, but got instead a dimension of " + << array.ndim() << "."; + throw std::domain_error(s.str()); + } + if ((array.shape(0) != n) || (array.shape(1) != m)) { + std::ostringstream s; + s << "error: expecting the given array to have the following shape (" << n << ", " << m + << "), but got instead the shape ("<< array.shape(0) << ", " << array.shape(1) << ")."; + throw std::domain_error(s.str()); + } + + // create raisim matrix + raisim::Mat mat; + + // copy the data + for (size_t i=0; i convert_vecdyn_to_np(const raisim::VecDyn &vec); + + +/// \brief: convert from np.array[float[n]] to raisim::VecDyn +raisim::VecDyn convert_np_to_vecdyn(py::array_t array); + + +/// \brief: convert from raisim::MatDyn to np.array[float64[n,m]] +py::array_t convert_matdyn_to_np(const raisim::MatDyn &mat); + + +/// \brief: convert from np.array[float[n,m]] to raisim::MatDyn +raisim::MatDyn convert_np_to_matdyn(py::array_t array); + + +#endif \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/articulated_system.cpp b/pyrobolearn/simulators/raisim_wrapper/src/articulated_system.cpp new file mode 100644 index 0000000..d31d8e6 --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/articulated_system.cpp @@ -0,0 +1,90 @@ +/** + * Python wrappers for raisim.object.ArticulatedSystem using pybind11. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types + +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc. +#include "raisim/World.hpp" + +namespace py = pybind11; +using namespace raisim; + + +void init_articulated_system(py::module &m) { + + + /****************/ + /* LoadFromMJCF */ + /****************/ +// py::class_(m, "LoadFromMJCF", "Load from MJCF file."); + + + /*****************/ + /* LoadFromURDF2 */ + /*****************/ +// py::class_(m, "LoadFromURDF2", "Load from URDF file."); + + + /***************/ + /* ControlMode */ + /***************/ + py::enum_(m, "Type", py::arithmetic()) + .value("FORCE_AND_TORQUE", raisim::ControlMode::Type::FORCE_AND_TORQUE) + .value("PD_PLUS_FEEDFORWARD_TORQUE", raisim::ControlMode::Type::PD_PLUS_FEEDFORWARD_TORQUE) + .value("VELOCITY_PLUS_FEEDFORWARD_TORQUE", raisim::ControlMode::Type::VELOCITY_PLUS_FEEDFORWARD_TORQUE); + + + /***************************/ + /* ArticulatedSystemOption */ + /***************************/ + py::class_(m, "ArticulatedSystemOption", "Articulated System Option.") + .def_readwrite("do_not_collide_with_parent", &raisim::ArticulatedSystemOption::doNotCollideWithParent); + + + /*********************/ + /* ArticulatedSystem */ + /*********************/ + py::class_ system(m, "ArticulatedSystem", "Raisim Articulated System."); + + system.def(py::init<>(), "Initialize the Articulated System.") + .def(py::init, raisim::ArticulatedSystemOption>(), + "Initialize the Articulated System." ) // TODO: finish the doc + .def("get_generalized_coordinate", [](raisim::ArticulatedSystem &self) { + return ; + }) + .def("update_kinematics", &raisim::ArticulatedSystem::updateKinematics, R"mydelimiter( + unnecessary to call this function if you are simulating your system. `integrate1` calls this function Call + this function if you want to get kinematic properties but you don't want to integrate. + )mydelimiter") + ; + + py::enum_(system, "Frame") + .value("WORLD_FRAME", raisim::ArticulatedSystem::Frame::WORLD_FRAME) + .value("PARENT_FRAME", raisim::ArticulatedSystem::Frame::PARENT_FRAME) + .value("BODY_FRAME", raisim::ArticulatedSystem::Frame::BODY_FRAME); + + +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/constraints.cpp b/pyrobolearn/simulators/raisim_wrapper/src/constraints.cpp new file mode 100644 index 0000000..9073151 --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/constraints.cpp @@ -0,0 +1,225 @@ +/** + * Python wrappers for raisim.constraints using pybind11. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types + +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc. +#include "raisim/object/Object.hpp" +#include "raisim/constraints/Constraints.hpp" +#include "raisim/constraints/Wire.hpp" +#include "raisim/constraints/StiffWire.hpp" +#include "raisim/constraints/CompliantWire.hpp" + +#include "converter.hpp" // contains code that allows to convert between the Vec, Mat to numpy arrays. + +namespace py = pybind11; +using namespace raisim; + + +void init_constraints(py::module &m) { + + + // create submodule + py::module constraints_module = m.def_submodule("constraints", "RaiSim contact submodule."); + + + /**************/ + /* Constraint */ + /**************/ + py::class_(constraints_module, "Constraints", "Raisim Constraints from which all other constraints inherit from."); + + +// +// /********/ +// /* Wire */ +// /********/ +// py::class_(constraints_module, "Wire", "Raisim Wire constraint class; it creates a wire constraint between 2 bodies.") +// .def("__init__", [](raisim::Wire &self, Object &object1, size_t local_idx1, py::array_t pos_body1, +// Object &object2, size_t local_idx2, py::array_t pos_body2, double length) +// { +// // convert the arrays to Vec<3> +// raisim::Vec<3> pos1 = convert_np_to_vec<3>(pos_body1); +// raisim::Vec<3> pos2 = convert_np_to_vec<3>(pos_body2); +// +// // instantiate the class +// new (&self) raisim::Wire(&object1, local_idx1, pos1, &object2, local_idx2, pos2, length); +// }, +// "Instantiate the wire constraint class.\n\n" +// "Args:\n" +// " object1 (Object): first object/body instance.\n" +// " local_idx1 (int): local index of the first object/body.\n" +// " pos_body1 (np.array[float[3]]): position of the constraint on the first body.\n" +// " object2 (Object): second object/body instance.\n" +// " local_idx2 (int): local index of the second object/body.\n" +// " pos_body2 (np.array[float[3]]): position of the constraint on the second body.\n" +// " length (float): length of the wire constraint.") +// +// +// .def("update", &raisim::Wire::update, "update internal variables (called by `integrate1()`).") +// +// +// .def("get_length", &raisim::Wire::getLength, R"mydelimiter( +// Get the length of the wire constraint. +// +// Returns: +// float: length of the wire constraint. +// )mydelimiter") +// +// +// .def("get_p1", [](raisim::Wire &self) { +// Vec<3> p1 = self.getP1(); +// return convert_vec_to_np(p1); +// }, R"mydelimiter( +// Return the first attachment point in the World frame. +// +// Returns: +// np.array[float[3]]: first point position expressed in the world frame. +// )mydelimiter") +// +// +// .def("get_p2", [](raisim::Wire &self) { +// Vec<3> p2 = self.getP2(); +// return convert_vec_to_np(p2); +// }, R"mydelimiter( +// Return the second attachment point in the World frame. +// +// Returns: +// np.array[float[3]]: second point position expressed in the world frame. +// )mydelimiter") +// +// +// .def("get_body1", &raisim::Wire::getBody1, R"mydelimiter( +// Return the first object to which the wire is attached. +// +// Returns: +// Object: first object. +// )mydelimiter") +// +// +// .def("get_body2", &raisim::Wire::getBody2, R"mydelimiter( +// Return the second object to which the wire is attached. +// +// Returns: +// Object: second object. +// )mydelimiter") +// +// +// .def("get_normal", [](raisim::Wire &self) { +// Vec<3> normal = self.getNorm(); +// return convert_vec_to_np(normal); +// }, R"mydelimiter( +// Return the direction of the normal (i.e., p2-p1 normalized) +// +// Returns: +// np.array[float[3]]: direction of the normal. +// )mydelimiter") +// +// +// .def("get_local_idx1", &raisim::Wire::getLocalIdx1, R"mydelimiter( +// Return the local index of object1. +// +// Returns: +// int: local index of object1. +// )mydelimiter") +// +// +// .def("get_local_idx2", &raisim::Wire::getLocalIdx2, R"mydelimiter( +// Return the local index of object2. +// +// Returns: +// int: local index of object2. +// )mydelimiter") +// +// +// .def("get_stretch", &raisim::Wire::getStretch, R"mydelimiter( +// Return the stretch length (i.e., constraint violation). +// +// Returns: +// float: stretch length. +// )mydelimiter") +// +// +// .def_property("name", &raisim::Wire::getName, &raisim::Object::setName) +// .def("get_name", &raisim::Wire::getName, "Get the wire constraint's name.") +// .def("set_name", &raisim::Wire::setName, "Set the wire constraint's name.", py::arg("name")) +// .def_readwrite("is_active", &raisim::Wire::isActive) +// ; +// +// +// /*************/ +// /* StiffWire */ +// /*************/ +// +// py::class_(constraints_module, "StiffWire", "Raisim StiffWire constraint class; it creates a stiff wire constraint between 2 bodies.") +// .def("__init__", [](raisim::StiffWire &self, Object &object1, size_t local_idx1, py::array_t pos_body1, +// Object &object2, size_t local_idx2, py::array_t pos_body2, double length) +// { +// // convert the arrays to Vec<3> +// raisim::Vec<3> pos1 = convert_np_to_vec<3>(pos_body1); +// raisim::Vec<3> pos2 = convert_np_to_vec<3>(pos_body2); +// +// // instantiate the class +// new (&self) raisim::StiffWire(&object1, local_idx1, pos1, &object2, local_idx2, pos2, length); +// }, +// "Instantiate the stiff wire constraint class.\n\n" +// "Args:\n" +// " object1 (Object): first object/body instance.\n" +// " local_idx1 (int): local index of the first object/body.\n" +// " pos_body1 (np.array[float[3]]): position of the constraint on the first body.\n" +// " object2 (Object): second object/body instance.\n" +// " local_idx2 (int): local index of the second object/body.\n" +// " pos_body2 (np.array[float[3]]): position of the constraint on the second body.\n" +// " length (float): length of the wire constraint."); +// +// +// /*****************/ +// /* CompliantWire */ +// /*****************/ +// +// py::class_(constraints_module, "CompliantWire", "Raisim Compliant Wire constraint class; it creates a compliant wire constraint between 2 bodies.") +// .def("__init__", [](raisim::CompliantWire &self, Object &object1, size_t local_idx1, py::array_t pos_body1, +// Object &object2, size_t local_idx2, py::array_t pos_body2, double length, double stiffness) +// { +// // convert the arrays to Vec<3> +// raisim::Vec<3> pos1 = convert_np_to_vec<3>(pos_body1); +// raisim::Vec<3> pos2 = convert_np_to_vec<3>(pos_body2); +// +// // instantiate the class +// new (&self) raisim::CompliantWire(&object1, local_idx1, pos1, &object2, local_idx2, pos2, length, stiffness); +// }, +// "Instantiate the compliant wire constraint class.\n\n" +// "Args:\n" +// " object1 (Object): first object/body instance.\n" +// " local_idx1 (int): local index of the first object/body.\n" +// " pos_body1 (np.array[float[3]]): position of the constraint on the first body.\n" +// " object2 (Object): second object/body instance.\n" +// " local_idx2 (int): local index of the second object/body.\n" +// " pos_body2 (np.array[float[3]]): position of the constraint on the second body.\n" +// " length (float): length of the wire constraint.\n" +// " stiffness (float): stiffness of the wire.") +// .def("apply_tension", &raisim::CompliantWire::applyTension, "Apply a tension in the compliant wire.") + +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/contact.cpp b/pyrobolearn/simulators/raisim_wrapper/src/contact.cpp new file mode 100644 index 0000000..42a3640 --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/contact.cpp @@ -0,0 +1,249 @@ +/** + * Python wrappers for raisim.contact using pybind11. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types + +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc. +#include "raisim/contact/Contact.hpp" +#include "raisim/contact/BisectionContactSolver.hpp" +#include "raisim/contact/PerObjectContactList.hpp" + +#include "converter.hpp" // contains code that allows to convert between the Vec, Mat to numpy arrays. + +namespace py = pybind11; +using namespace raisim; + + +void init_contact(py::module &m) { + + + // create submodule + py::module contact_module = m.def_submodule("contact", "RaiSim contact submodule."); + + + /*****************/ + /* Contact class */ + /*****************/ + py::class_(contact_module, "Contact", "Raisim Contact.") + .def("__init__", [](raisim::contact::Contact &self, py::array_t position, py::array_t normal, + bool objectA, size_t contact_problem_index, size_t contact_index_in_object, size_t pair_object_index, + BodyType pair_object_body_type, size_t pair_contact_index_in_pair_object, size_t local_body_index, + double depth) + { + // convert the arrays to Vec<3> + raisim::Vec<3> pos = convert_np_to_vec<3>(position); + raisim::Vec<3> norm = convert_np_to_vec<3>(normal); + + // instantiate the class + new (&self) raisim::contact::Contact(pos, norm, objectA, contact_problem_index, contact_index_in_object, + pair_object_index, pair_object_body_type, pair_contact_index_in_pair_object, local_body_index, depth); + }, + "Instantiate the contact class.\n\n" + "Args:\n" + " position (np.array[float[3]]): position vector.\n" + " normal (np.array[float[3]]): normal vector.\n" + " objectA (bool): True if object A.\n" + " contact_problem_index (int): contact problem index.\n" + " contact_index_in_object (int): contact index in object (an object can be in contact at multiple points).\n" + " pair_object_index (int): pair object index.\n" + " pair_object_index (BodyType): pair object body type between {STATIC, KINEMATIC, DYNAMIC}.\n" + " pair_contact_index_in_pair_object (int): pair contact index in pair object.\n" + " local_body_index (int): local body index." + " depth (float): depth of the contact.") + + + .def("get_position", [](raisim::contact::Contact &self) { + Vec<3> position = self.getPosition(); + return convert_vec_to_np(position); + }, R"mydelimiter( + Get the contact position. + + Returns: + np.array[float[3]]: contact position in the world. + )mydelimiter") + + + .def("get_normal", [](raisim::contact::Contact &self) { + Vec<3> normal = self.getNormal(); + return convert_vec_to_np(normal); + }, R"mydelimiter( + Get the contact normal. + + Returns: + np.array[float[3]]: contact normal in the world. + )mydelimiter") + + + .def("get_contact_frame", [](raisim::contact::Contact &self) { + Mat<3, 3> frame = self.getContactFrame(); + return convert_mat_to_np(frame); + }, R"mydelimiter( + Get the contact frame. + + Returns: + np.array[float[3, 3]]: contact frame. + )mydelimiter") + + + .def("get_index_contact_problem", &raisim::contact::Contact::getIndexContactProblem, R"mydelimiter( + Get the index contact problem. + + Returns: + int: index. + )mydelimiter") + + + .def("get_pair_object_index", &raisim::contact::Contact::getPairObjectIndex, R"mydelimiter( + Get the pair object index. + + Returns: + int: index. + )mydelimiter") + + + .def("get_pair_contact_index_in_pair_object", &raisim::contact::Contact::getPairContactIndexInPairObject, R"mydelimiter( + Get the pair contact index in pair objects. + + Returns: + int: index. + )mydelimiter") + + + .def("get_impulse", [](raisim::contact::Contact &self) { + Vec<3> *impulse = self.getImpulse(); + return convert_vec_to_np(*impulse); + }, R"mydelimiter( + Get the impulse. + + Returns: + np.array[float[3]]: impulse. + )mydelimiter") + + + .def("is_objectA", &raisim::contact::Contact::isObjectA, R"mydelimiter( + Check if it is object A. + + Returns: + bool: True if object A is in contact. + )mydelimiter") + + + .def("get_pair_object_body_type", &raisim::contact::Contact::getPairObjectBodyType, R"mydelimiter( + Get the pair object body type. + + Returns: + raisim.BodyType: the body type (STATIC, KINEMATIC, DYNAMIC) + )mydelimiter") + + + .def("set_impulse", [](raisim::contact::Contact &self, py::array_t impulse) { + Vec<3> impulse_ = convert_np_to_vec<3>(impulse); + self.setImpulse(&impulse_); + }, R"mydelimiter( + Set the impulse. + + Args: + np.array[float[3]]: impulse. + )mydelimiter", + py::arg("impulse")) + + + .def("set_inverse_inertia", [](raisim::contact::Contact &self, py::array_t inverse_inertia) { + Mat<3, 3> I_ = convert_np_to_mat<3, 3>(inverse_inertia); + self.setInvInertia(&I_); + }, R"mydelimiter( + Set the inverse of the inertia matrix. + + Args: + np.array[float[3,3]]: inverse of the inertia matrix. + )mydelimiter", + py::arg("inverse_inertia")) + + + .def("get_inverse_inertia", [](raisim::contact::Contact &self) { + const Mat<3, 3> *I_ = self.getInvInertia(); + return convert_mat_to_np(*I_); + }, R"mydelimiter( + Get the inverse inertia matrix. + + Returns: + np.array[float[3,3]]: inverse of the inertia matrix. + )mydelimiter") + + + .def("get_local_body_index", &raisim::contact::Contact::getlocalBodyIndex, R"mydelimiter( + Get local body index. + + Returns: + int: local body index. + )mydelimiter") + + + .def("get_depth", &raisim::contact::Contact::getDepth, R"mydelimiter( + Get the depth. + + Returns: + float: depth. + )mydelimiter") + + + .def("is_self_collision", &raisim::contact::Contact::isSelfCollision, R"mydelimiter( + Return True if self-collision is enabled. + + Returns: + bool: True if self-collision is enabled. + )mydelimiter") + + + .def("set_self_collision", &raisim::contact::Contact::setSelfCollision, "Enable self-collision.") + + + .def("skip", &raisim::contact::Contact::skip, R"mydelimiter( + Return True if we contact is skipped. + + Returns: + bool: True if the contact is skipped. + )mydelimiter") + + + .def("set_skip", &raisim::contact::Contact::setSkip, "Skip this contact."); + + + /**************************/ + /* BisectionContactSolver */ + /**************************/ + + py::class_(contact_module, "Single3DContactProblem", "Raisim single 3D contact problem.") + .def(py::init<>(), "Initialize the single 3D contact problem.") + .def(py::init()) + .def("check_rank", &raisim::contact::Single3DContactProblem::checkRank) + ; + + /************************/ + /* PerObjectContactList */ + /************************/ + +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/converter.cpp b/pyrobolearn/simulators/raisim_wrapper/src/converter.cpp new file mode 100644 index 0000000..f8f49ae --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/converter.cpp @@ -0,0 +1,106 @@ +/** + * Type converters used to convert between different data types. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "converter.hpp" + +namespace py = pybind11; + + +/// \brief: convert from raisim::VecDyn to np.array[float64[n]] +py::array_t convert_vecdyn_to_np(const raisim::VecDyn &vec) { + const double *ptr = vec.ptr(); // get data pointer + size_t n = vec.n; // get dimension + + // return np.array[float64[n,m]] + return py::array_t( + {n}, // shape + {sizeof(double)}, // C-style contiguous strides for double (double=8bytes) + ptr); +// vec); // numpy array references this parent +} + + +/// \brief: convert from np.array[float[n]] to raisim::VecDyn +raisim::VecDyn convert_np_to_vecdyn(py::array_t array) { + + size_t size = array.size(); + + // reshape if necessary + if (array.ndim() > 1) + array.resize({size}); + + // create raisim dynamic vector + raisim::VecDyn vec(size); + + // copy the data + for(size_t i=0; i convert_matdyn_to_np(const raisim::MatDyn &mat) { + const double *ptr = mat.ptr(); // get data pointer + size_t n = mat.n; + size_t m = mat.m; + + // return np.array[float64[n,m]] + return py::array_t( + {n, m}, // shape + {sizeof(double), sizeof(double)}, // C-style contiguous strides for double (double=8bytes) + ptr); +// mat); // numpy array references this parent +} + + +/// \brief: convert from np.array[float[n,m]] to raisim::MatDyn +raisim::MatDyn convert_np_to_matdyn(py::array_t array) { + + // check dimensions and shape + if (array.ndim() != 2) { + std::ostringstream s; + s << "error: expecting the given array to have a dimension of 2, but got instead a dimension of " + << array.ndim() << "."; + throw std::domain_error(s.str()); + } + + // get the number of rows and columns + size_t nrows = array.shape(0); + size_t ncols = array.shape(1); + + // create raisim matrix + raisim::MatDyn mat(nrows, ncols); + + // copy the data + for (size_t i=0; i + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types + +#include "raisim/object/Object.hpp" + +namespace py = pybind11; +using namespace raisim; + + +void init_single_bodies(py::module &); +void init_articulated_system(py::module &); +void init_terrain(py::module &); + + +void init_object(py::module &m) { + + // create submodule + py::module object_module = m.def_submodule("object", "RaiSim contact submodule."); + + + /**************/ + /* ObjectType */ + /**************/ + // object type enum (from include/raisim/configure.hpp) + py::enum_(object_module, "ObjectType", py::arithmetic()) + .value("SPHERE", raisim::ObjectType::SPHERE) + .value("BOX", raisim::ObjectType::BOX) + .value("CYLINDER", raisim::ObjectType::CYLINDER) + .value("CONE", raisim::ObjectType::CONE) + .value("CAPSULE", raisim::ObjectType::CAPSULE) + .value("MESH", raisim::ObjectType::MESH) + .value("HALFSPACE", raisim::ObjectType::HALFSPACE) + .value("COMPOUND", raisim::ObjectType::COMPOUND) + .value("HEIGHTMAP", raisim::ObjectType::HEIGHTMAP) + .value("ARTICULATED_SYSTEM", raisim::ObjectType::ARTICULATED_SYSTEM); + + + /************/ + /* BodyType */ + /************/ + // body type enum (from include/raisim/configure.hpp) + py::enum_(object_module, "BodyType", py::arithmetic()) + .value("STATIC", raisim::BodyType::STATIC) + .value("KINEMATIC", raisim::BodyType::KINEMATIC) + .value("DYNAMIC", raisim::BodyType::DYNAMIC); + + + /**********/ + /* Object */ + /**********/ + py::class_(object_module, "Object", "Raisim Object from which all other objects/bodies inherit from.") + .def_property("name", &raisim::Object::getName, &raisim::Object::setName) + .def("get_name", &raisim::Object::getName, "Get the object's name.") + .def("set_name", &raisim::Object::setName, "Set the object's name.", py::arg("name")) + .def("clear_per_object_contact", &raisim::Object::clearPerObjectContact) + .def("add_contact_to_per_object_contact", &raisim::Object::addContactToPerObjectContact) + ; + + + // raisim.object.singleBodies + init_single_bodies(object_module); + + // raisim.object.ArticulatedSystem + init_articulated_system(object_module); + + // raisim.object.terrain + init_terrain(object_module); + +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/raisim_wrapper.cpp b/pyrobolearn/simulators/raisim_wrapper/src/raisim_wrapper.cpp new file mode 100644 index 0000000..899084d --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/raisim_wrapper.cpp @@ -0,0 +1,309 @@ +/** + * Python wrappers for RaiSim using pybind11. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types + +#include + +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc. +#include "raisim/World.hpp" +#include "raisim/RaisimServer.hpp" +#include "raisim/OgreVis.hpp" +//#include "visualizer/raisimKeyboardCallback.hpp" +//#include "visualizer/helper.hpp" +//#include "visualizer/guiState.hpp" +//#include "visualizer/raisimBasicImguiPanel.hpp" + +#include "converter.hpp" // contains code that allows to convert between the Vec, Mat to numpy arrays. + +namespace py = pybind11; +using namespace raisim; + + +void init_object(py::module &); +void init_constraints(py::module &); +void init_contact(py::module &); +// void init_visualizer(py::module &); + + +// The PYBIND11_MODULE() macro creates a function that will be called when an import statement is issued from within +// Python. In the following, "raisim" is the module name, "m" is a variable of type py::module which is the main +// interface for creating bindings. The method module::def() generates binding code that exposes the C++ function +// to Python. +PYBIND11_MODULE(raisimpy, m) { + + m.doc() = "Python wrappers for the RaiSim library and visualizer."; // docstring for the module + + + /*************/ + /* Materials */ + /*************/ + py::class_(m, "MaterialPairProperties", "Raisim Material Pair Properties (friction and restitution).") + .def(py::init<>(), "Initialize the material pair properties.") + .def(py::init(), + "Initialize the material pair properties.\n\n" + "Args:\n" + " friction (float): coefficient of friction.\n" + " restitution (float): coefficient of restitution.\n" + " threshold (float): restitution threshold.", + py::arg("friction"), py::arg("restitution"), py::arg("threshold")); + + + py::class_(m, "MaterialManager", "Raisim Material Manager.") + .def(py::init<>(), "Initialize the material pair manager.") + .def(py::init(), + "Initialize the material manager by uploading the material data from a file.\n\n" + "Args:\n" + " xml_file (float): xml file.", + py::arg("xml_file")) + .def("set_material_pair_properties", &raisim::MaterialManager::setMaterialPairProp, R"mydelimiter( + Set the material pair properties (friction and restitution). + + Args: + material1 (str): first material. + material2 (str): second material. + friction (float): coefficient of friction. + restitution (float): coefficient of restitution. + threshold (float): restitution threshold. + )mydelimiter", + py::arg("material1"), py::arg("material2"), py::arg("friction"), py::arg("restitution"), py::arg("threshold")) + .def("get_material_pair_properties", &raisim::MaterialManager::getMaterialPairProp, R"mydelimiter( + Get the material pair properties (friction and restitution). + + Args: + material1 (str): first material. + material2 (str): second material. + + Returns: + MaterialPairProperties: material pair properties (friction, restitution, and restitution threshold). + )mydelimiter", + py::arg("material1"), py::arg("material2")) + .def("set_default_material_properties", &raisim::MaterialManager::setDefaultMaterialProperties, R"mydelimiter( + Set the default material properties. + + Args: + friction (float): coefficient of friction. + restitution (float): coefficient of restitution. + threshold (float): restitution threshold. + )mydelimiter", + py::arg("friction"), py::arg("restitution"), py::arg("threshold")) + ; + + + /******************/ + /* raisim.contact */ + /******************/ + init_contact(m); + + /*****************/ + /* raisim.object */ + /*****************/ + init_object(m); // define primitive shapes and articulated systems) + + /*********************/ + /* raisim.constraint */ + /*********************/ + init_constraints(m); + + /*********/ + /* World */ + /*********/ + py::class_(m, "World", "Raisim world.", py::dynamic_attr()) // enable dynamic attributes for C++ class in Python + .def(py::init<>(), "Initialize the World.") + .def(py::init(), "Initialize the World from the given config file.", py::arg("configFile")) + .def("set_time_step", &raisim::World::setTimeStep, R"mydelimiter( + Set the given time step `dt` in the simulator. + + Args: + dt (float): time step to be set in the simulator. + )mydelimiter", + py::arg("dt")) + .def("get_time_step", &raisim::World::getTimeStep, R"mydelimiter( + Get the current time step that has been set in the simulator. + + Returns: + float: time step. + )mydelimiter") + +// .def("add_sphere", &raisim::World::addSphere, R"mydelimiter( +// Add dynamically a sphere into the world. +// +// Args: +// radius (float): radius of the sphere. +// mass (float): mass of the sphere. +// material (str): material to be applied to the sphere. +// collision_group (unsigned long): collision group. +// collision_mask (unsigned long): collision mask. +// Returns: +// Sphere: the sphere instance. +// )mydelimiter", +// py::arg("radius"), py::arg("mass"), py::arg("material") = "default", py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1)) +// .def("add_box", &raisim::World::addBox, R"mydelimiter( +// Add dynamically a box into the world. +// +// Args: +// x (float): length along the x axis. +// y (float): length along the y axis. +// z (float): length along the z axis. +// mass (float): mass of the box. +// material (str): material to be applied to the box. +// collision_group (unsigned long): collision group. +// collision_mask (unsigned long): collision mask. +// Returns: +// Box: the box instance. +// )mydelimiter", +// py::arg("x"), py::arg("y"), py::arg("z"), py::arg("mass"), py::arg("material") = "default", py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1)) +// .def("add_cylinder", &raisim::World::addCylinder, R"mydelimiter( +// Add dynamically a cylinder into the world. +// +// Args: +// radius (float): radius of the cylinder. +// height (float): height of the cylinder. +// mass (float): mass of the cylinder. +// material (str): material to be applied to the cylinder. +// collision_group (unsigned long): collision group. +// collision_mask (unsigned long): collision mask. +// Returns: +// Cylinder: the cylinder instance. +// )mydelimiter", +// py::arg("radius"), py::arg("height"), py::arg("mass"), py::arg("material") = "default", py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1)) +// .def("add_cone", &raisim::World::addCone, R"mydelimiter( +// Add dynamically a cone into the world. +// +// Args: +// radius (float): radius of the cone. +// height (float): height of the cone. +// mass (float): mass of the cone. +// material (str): material to be applied to the cone. +// collision_group (unsigned long): collision group. +// collision_mask (unsigned long): collision mask. +// Returns: +// Cone: the cone instance. +// )mydelimiter", +// py::arg("radius"), py::arg("height"), py::arg("mass"), py::arg("material") = "default", py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1)) +// .def("add_capsule", &raisim::World::addCapsule, R"mydelimiter( +// Add dynamically a capsule into the world. +// +// Args: +// radius (float): radius of the capsule. +// height (float): height of the capsule. +// mass (float): mass of the capsule. +// material (str): material to be applied to the capsule. +// collision_group (unsigned long): collision group. +// collision_mask (unsigned long): collision mask. +// Returns: +// Capsule: the capsule instance. +// )mydelimiter", +// py::arg("radius"), py::arg("height"), py::arg("mass"), py::arg("material") = "default", py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1)) +// .def("add_ground", &raisim::World::addGround, R"mydelimiter( +// Add dynamically a ground into the world. +// +// Args: +// height (float): height of the ground. +// material (str): material to be applied to the ground. +// collision_mask (unsigned long): collision mask. +// Returns: +// Ground: the ground instance. +// )mydelimiter", +// py::arg("height"), py::arg("material") = "default", py::arg("collision_mask") = CollisionGroup(-1)) + +// .def("add_heightmap", &raisim::World::, R"mydelimiter( +// Add dynamically a ground into the world. +// +// Args: +// height (float): height of the ground. +// material (str): material to be applied to the ground. +// collision_mask (unsigned long): collision mask. +// Returns: +// Ground: the ground instance. +// )mydelimiter", +// py::arg("height"), py::arg("material") = "default", py::arg("collision_mask") = CollisionGroup(-1)) + + + .def("integrate", &raisim::World::integrate, "this function is simply calling both `integrate1()` and `integrate2()` one-by-one.") + .def("integrate1", &raisim::World::integrate1, R"mydelimiter( + It performs: + 1. deletion contacts from previous time step + 2. collision detection + 3. register contacts to each body + 4. calls `preContactSolverUpdate1()` of each object + )mydelimiter") + .def("integrate2", &raisim::World::integrate2, R"mydelimiter( + It performs + 1. calls `preContactSolverUpdate2()` of each body + 2. run collision solver + 3. calls `integrate` method of each object + )mydelimiter") + .def("get_gravity", [](raisim::World &world) { + Vec<3> gravity = world.getGravity(); + return convert_vec_to_np(gravity); + }, R"mydelimiter( + Get the gravity vector from the world. + + Returns: + np.array[float[3]]: gravity vector. + )mydelimiter") + .def("set_gravity", [](raisim::World &world, py::array_t array) { + raisim::Vec<3> gravity = convert_np_to_vec<3>(array); + world.setGravity(gravity); + }, R"mydelimiter( + Set the gravity vector in the world. + + Args: + np.array[float[3]]: gravity vector. + )mydelimiter", py::arg("gravity")) + .def("set_erp", &raisim::World::setERP, "Set the error reduction parameter (ERP).", py::arg("erp"), py::arg("erp2")=0) + .def("set_contact_solver_parameters", &raisim::World::setContactSolverParam, R"mydelimiter( + Set contact solver parameters. + + Args: + alpha_init (float): alpha init. + alpha_min (float): alpha minimum. + alpha_decay (float): alpha decay. + max_iters (float): maximum number of iterations. + threshold (float): threshold. + )mydelimiter", + py::arg("alpha_init"), py::arg("alpha_min"), py::arg("alpha_decay"), py::arg("max_iters"), py::arg("threshold")) + .def("get_world_time", &raisim::World::getWorldTime, R"mydelimiter( + Return the total integrated time (which is updated at every `integrate2()`` call). + + Returns: + float: world time. + )mydelimiter") + .def("set_world_time", &raisim::World::setWorldTime, R"mydelimiter( + Set the world time. + + Args: + time (float): world time + )mydelimiter", py::arg("time")) + ; + + // visualizer class +// py::class_>(m, "Visualizer", "Ogre visualizer for Raisim.") +// .def(py::init(&raisim::OgreVis::get), "Create Ogre visualizer instance (singleton).", py::return_value_policy::reference) +// .def("get", &raisim::OgreVis::get, "Get the single Ogre visualizer instance (singleton).") +// .def(); +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/single_bodies.cpp b/pyrobolearn/simulators/raisim_wrapper/src/single_bodies.cpp new file mode 100644 index 0000000..23c22ab --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/single_bodies.cpp @@ -0,0 +1,272 @@ +/** + * Python wrappers for raisim.object using pybind11. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types +//#include // numpy types + +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc. +#include "raisim/object/singleBodies/Box.hpp" +#include "raisim/object/singleBodies/Capsule.hpp" +#include "raisim/object/singleBodies/Compound.hpp" +#include "raisim/object/singleBodies/Cone.hpp" +#include "raisim/object/singleBodies/Cylinder.hpp" +#include "raisim/object/singleBodies/Mesh.hpp" +#include "raisim/object/singleBodies/SingleBodyObject.hpp" +#include "raisim/object/singleBodies/Sphere.hpp" + +#include "converter.hpp" // contains code that allows to convert between the Vec, Mat to numpy arrays. + +namespace py = pybind11; +using namespace raisim; + + +void init_single_bodies(py::module &m) { + + /********************/ + /* SingleBodyObject */ + /********************/ + py::class_(m, "SingleBodyObject", "Raisim Single Object from which all single objects/bodies (such as box, sphere, etc) inherit from.") + .def(py::init(), "Initialize the Object.", py::arg("object_type")) + .def("get_position", &raisim::SingleBodyObject::getPosition, R"mydelimiter( + Get the body's position with respect to the world frame. + + Returns: + np.array[float[3]]: position in the world frame. + )mydelimiter") + .def("get_com_position", &raisim::SingleBodyObject::getComPosition, R"mydelimiter( + Get the body's center of mass position with respect to the world frame. + + Returns: + np.array[float[3]]: center of mass position in the world frame. + )mydelimiter") + .def("get_linear_velocity", &raisim::SingleBodyObject::getLinearVelocity, R"mydelimiter( + Get the body's linear velocity with respect to the world frame. + + Returns: + np.array[float[3]]: linear velocity in the world frame. + )mydelimiter") + .def("get_angular_velocity", &raisim::SingleBodyObject::getAngularVelocity, R"mydelimiter( + Get the body's angular velocity position with respect to the world frame. + + Returns: + np.array[float[3]]: angular velocity in the world frame. + )mydelimiter") + .def("get_quaternion", py::overload_cast<>(&raisim::SingleBodyObject::getQuaternion), R"mydelimiter( + Get the body's orientation (expressed as a quaternion [w,x,y,z]) with respect to the world frame. + + Returns: + np.array[float[4]]: quaternion [w,x,y,z]. + )mydelimiter") + .def("get_rotation_matrix", py::overload_cast<>(&raisim::SingleBodyObject::getRotationMatrix), R"mydelimiter( + Get the body's orientation (expressed as a rotation matrix) with respect to the world frame. + + Returns: + np.array[float[3,3]]: rotation matrix. + )mydelimiter") + .def("get_kinetic_energy", &raisim::SingleBodyObject::getKineticEnergy, R"mydelimiter( + Get the body's kinetic energy. + + Returns: + float: kinetic energy. + )mydelimiter") + .def("get_potential_energy", &raisim::SingleBodyObject::getPotentialEnergy, R"mydelimiter( + Get the body's potential energy. + + Returns: + float: potential energy. + )mydelimiter") + .def("get_energy", &raisim::SingleBodyObject::getEnergy, R"mydelimiter( + Get the body's total energy. + + Returns: + float: total energy. + )mydelimiter") + .def("get_linear_momentum", &raisim::SingleBodyObject::getLinearMomentum, R"mydelimiter( + Get the body's linear momentum. + + Returns: + np.array[float[3]]: linear momentum. + )mydelimiter") + .def("get_mass", &raisim::SingleBodyObject::getMass, R"mydelimiter( + Get the body's mass. + + Returns: + float: mass (kg). + )mydelimiter") + .def("get_world_inertia_matrix", &raisim::SingleBodyObject::getInertiaMatrix_W, R"mydelimiter( + Get the body's inertia matrix expressed in the world frame. + + Returns: + np.array[float[3,3]]: world inertia matrix. + )mydelimiter") + .def("get_body_inertia_matrix", &raisim::SingleBodyObject::getInertiaMatrix_B, R"mydelimiter( + Get the body's inertia matrix expressed in the body frame. + + Returns: + np.array[float[3,3]]: body inertia matrix. + )mydelimiter") + .def("get_object_type", &raisim::SingleBodyObject::getObjectType, R"mydelimiter( + Get the body's type. + + Returns: + raisim.ObjectType: object type (BOX, CYLINDER, CAPSULE, CONE, SPHERE, etc.) + )mydelimiter") + ; + + + /*******/ + /* Box */ + /*******/ + py::class_(m, "Box", "Raisim Box.") + .def(py::init(), + "Initialize a box.\n\n" + "Args:\n" + " x (float): length along the x axis.\n" + " y (float): length along the y axis.\n" + " z (float): length along the z axis.\n" + " mass (float): mass of the box.", + py::arg("x"), py::arg("y"), py::arg("z"), py::arg("mass")) + .def("get_dimensions", [](raisim::Box &box) { + Vec<3> dimensions = box.getDim(); + return convert_vec_to_np(dimensions); + }, R"mydelimiter( + Get the box's dimensions. + + Returns: + tuple[float[3]]: dimensions along each axis. + )mydelimiter"); + + + /***********/ + /* Capsule */ + /***********/ + py::class_(m, "Capsule", "Raisim Capsule.") + .def(py::init(), + "Initialize a capsule.\n\n" + "Args:\n" + " radius (float): radius of the capsule.\n" + " height (float): height of the capsule.\n" + " mass (float): mass of the capsule.", + py::arg("radius"), py::arg("height"), py::arg("mass")) + .def("get_radius", &raisim::Capsule::getRadius, R"mydelimiter( + Get the capsule's radius. + + Returns: + float: radius of the capsule. + )mydelimiter") + .def("get_height", &raisim::Capsule::getHeight, R"mydelimiter( + Get the capsule's height. + + Returns: + float: height of the capsule. + )mydelimiter"); + + + /************/ + /* Compound */ + /************/ + + + /********/ + /* Cone */ + /********/ + py::class_(m, "Cone", "Raisim Cone.") + .def(py::init(), + "Initialize a cone.\n\n" + "Args:\n" + " radius (float): radius of the cone.\n" + " height (float): height of the cone.\n" + " mass (float): mass of the cone.", + py::arg("radius"), py::arg("height"), py::arg("mass")) + .def("get_radius", &raisim::Cone::getRadius, R"mydelimiter( + Get the cone's radius. + + Returns: + float: radius of the cone. + )mydelimiter") + .def("get_height", &raisim::Cone::getHeight, R"mydelimiter( + Get the cone's height. + + Returns: + float: height of the cone. + )mydelimiter"); + + + /************/ + /* Cylinder */ + /************/ + py::class_(m, "Cylinder", "Raisim Cylinder.") + .def(py::init(), + "Initialize a cylinder.\n\n" + "Args:\n" + " radius (float): radius of the cylinder.\n" + " height (float): height of the cylinder.\n" + " mass (float): mass of the cylinder.", + py::arg("radius"), py::arg("height"), py::arg("mass")) + .def("get_radius", &raisim::Cylinder::getRadius, R"mydelimiter( + Get the cylinder's radius. + + Returns: + float: radius of the cylinder. + )mydelimiter") + .def("get_height", &raisim::Cylinder::getHeight, R"mydelimiter( + Get the cylinder's height. + + Returns: + float: height of the cylinder. + )mydelimiter"); + + + /********/ + /* Mesh */ + /********/ +// py::class_(m, "Mesh", "Raisim Mesh.") +// .def(py::init(), +// "Initialize a Mesh.\n\n" +// "Args:\n" +// " filename (str): path to the mesh file.\n" +// " space (dSpaceID): space.", +// py::arg("filename"), py::arg("space")); + + + /**********/ + /* Sphere */ + /**********/ + py::class_(m, "Sphere", "Raisim Sphere.") + .def(py::init(), + "Initialize a sphere.\n\n" + "Args:\n" + " radius (float): radius of the sphere.\n" + " mass (float): mass of the sphere.", + py::arg("radius"), py::arg("mass")) + .def("get_radius", &raisim::Sphere::getRadius, R"mydelimiter( + Get the sphere's radius. + + Returns: + float: radius of the sphere. + )mydelimiter"); + +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/terrain.cpp b/pyrobolearn/simulators/raisim_wrapper/src/terrain.cpp new file mode 100644 index 0000000..2caffee --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/terrain.cpp @@ -0,0 +1,44 @@ +/** + * Python wrappers for raisim.object.terrain using pybind11. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types + +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc. +#include "raisim/object/terrain/Ground.hpp" +#include "raisim/object/terrain/HeightMap.hpp" + +namespace py = pybind11; +using namespace raisim; + + +void init_terrain(py::module &m) { + + // ground class + + // heightmap class + + +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/src/visualizer.cpp b/pyrobolearn/simulators/raisim_wrapper/src/visualizer.cpp new file mode 100644 index 0000000..9638635 --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/src/visualizer.cpp @@ -0,0 +1,48 @@ +/** + * Python wrappers for raisimOgre using pybind11. + * + * Copyright (c) 2019, Brian Delhaisse + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict +#include // automatic conversion between Eigen data types to Numpy data types + +#include + +#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc. +#include "raisim/World.hpp" +#include "raisim/RaisimServer.hpp" +#include "raisim/OgreVis.hpp" +//#include "visualizer/raisimKeyboardCallback.hpp" +//#include "visualizer/helper.hpp" +//#include "visualizer/guiState.hpp" +//#include "visualizer/raisimBasicImguiPanel.hpp" + + +namespace py = pybind11; +using namespace raisim; + + +void init_visualizer(py::module &m) { + + +} \ No newline at end of file diff --git a/pyrobolearn/simulators/raisim_wrapper/test.py b/pyrobolearn/simulators/raisim_wrapper/test.py new file mode 100644 index 0000000..e7432ae --- /dev/null +++ b/pyrobolearn/simulators/raisim_wrapper/test.py @@ -0,0 +1,21 @@ +import numpy as np +import raisimpy as raisim + +world = raisim.World() +print("gravity: ", world.get_gravity()) +print("set gravity to: np.array([0.,1.,2.])") +world.set_gravity(np.array([0.,1.,2.])) +print("gravity: ", world.get_gravity()) +print("set gravity to: np.array([0.,-1.,-2.]).reshape(-1,1)") +world.set_gravity(np.array([0.,-1.,-2.]).reshape(-1,1)) +print("gravity: ", world.get_gravity()) +print("set gravity to: np.array([0.,1.,2.]).reshape(1,-1)") +world.set_gravity(np.array([0.,1.,2.]).reshape(1,-1)) +print("gravity: ", world.get_gravity()) +print("set gravity to: range(3,6)") +world.set_gravity(range(3,6)) +print("gravity: ", world.get_gravity()) + +print("set gravity to: range(3,7)") +world.set_gravity(range(3,7)) +