wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

update simulator and move python wrappers in another repo (raisimpy)

Browse Source
This commit is contained in:
Brian Delhaisse
2019-08-03 02:17:32 +02:00
parent 7ca3281c05
commit 586033304a
35 changed files with 4 additions and 6148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/simulators/raisim.py
+4 -4
View File
@@ -3,8 +3,8 @@
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].
by RaiSim. Because it didn't have a Python wrapper, one has been written using pybind11 [6]. This wrapper ``raisimpy``
can be found in the following repository: https://github.com/robotlearn/raisimpy
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.
@@ -23,14 +23,14 @@ References:
- [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
- [9] raisimpy: https://github.com/robotlearn/raisimpy
"""
# 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.")
print(e.__str__() + "\nHINT: you need to install `raisimLib`, `raisimOgre`, and `raisimpy")
# import PRL simulator
from pyrobolearn.simulators.simulator import Simulator
pyrobolearn/simulators/raisim_wrapper/CMakeLists.txt
-25
View File
@@ -1,25 +0,0 @@
# Run: cmake -DPYBIND11_PYTHON_VERSION=<PYTHON_VERSION> -DCMAKE_PREFIX_PATH=$LOCAL_BUILD ..
# where <PYTHON_VERSION>=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)
pyrobolearn/simulators/raisim_wrapper/LICENSE
-23
View File
@@ -1,23 +0,0 @@
MIT License
Copyright (c) 2019, Robotic Systems Lab, ETH Zurich (Original C++ code by Hwangbo et al.)
Copyright (c) 2019, Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
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.
pyrobolearn/simulators/raisim_wrapper/README.rst
-132
View File
@@ -1,132 +0,0 @@
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 <https://github.com/leggedrobotics/raisimLib>`_ and
`raisimOgre <https://github.com/leggedrobotics/raisimOgre>`_. You will also have to install
`pybind11 <https://pybind11.readthedocs.io/en/stable/>`_ as we use this to wrap the C++ code.
If you followed the installation procedure of ``raisimLib`` and/or ``raisimOgre``, you will have the two following
environment variables defined:
- WORKSPACE: workspace where you clone your git repos (e.g., ~/raisim_workspace)
- LOCAL_BUILD: build directory where you install exported cmake libraries (e.g., ~/raisim_build)
Before compiling the code in this repo, you will have to move or copy the `extras` folder (that you can find in this
repo) in the `$LOCAL_BUILD/include/ode/` folder. This `extras` folder contains some missing header files for ODE which
are necessary in order, for instance, to load meshes with Raisim. This can be done by:
.. code-block:: bash
cp -r extras $LOCAL_BUILD/include/ode/
Now, you can finally compile the python wrappers 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`` is the Python version you wish to use. For instance, ``PYTHON_VERSION=3.5``.
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))
We follow mostly the naming convention defined in ``raisimLib`` and ``raisimOgre``, however we follow the PEP8 guideline.
Thus, a C++ method like:
.. code-block:: cpp
getComPosition()
becomes
.. code-block:: python
get_com_position()
Note that in the original ``raisimLib``, the authors sometimes use their own defined data types for vectors and
matrices (such as ``Vec<n>``, ``Mat<n,m>``, ``VecDyn``, and ``MatDyn``). When using the python wrappers, these
datatypes are converted back and forth to numpy arrays as this is the standard in Python.
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 <Eigen/*>`` by
``#include <eigen3/Eigen/*>``. 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 <Eigen/*>`` by ``#include <eigen3/Eigen/*>``.
Citation
~~~~~~~~
If the code presented here was useful to you, we would appreciate if you could cite the original authors:
.. code-block:: latex
@article{hwangbo2018per,
title={Per-contact iteration method for solving contact dynamics},
author={Hwangbo, Jemin and Lee, Joonho and Hutter, Marco},
journal={IEEE Robotics and Automation Letters},
volume={3},
number={2},
pages={895--902},
year={2018},
publisher={IEEE}
}
If you still have some space in your paper for the references, you can add the following citation:
.. code-block::
@misc{delhaisse2019raisimpy
author = {Delhaisse, Brian},
title = {RaiSimPy: A Python wrapper for RaiSim},
howpublished = {\url{https://github.com/robotlearn/raisimpy}},
year=2019,
}
Otherwise, you can just add me in the acknowledgements ;)
If you use ``raisimpy`` through the `pyrobolearn <https://github.com/robotlearn/pyrobolearn>`_ framework, you can cite
this last one instead (but you still have to cite the authors of Raisim).
pyrobolearn/simulators/raisim_wrapper/extras/LICENSE
-13
View File
@@ -1,13 +0,0 @@
Open Dynamics Engine
Copyright (c) 2001-2004, Russell L. Smith.
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
Neither the names of ODE's copyright owner nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
pyrobolearn/simulators/raisim_wrapper/extras/array.h
-135
View File
@@ -1,135 +0,0 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/* this comes from the `reuse' library. copy any changes back to the source.
*
* Variable sized array template. The array is always stored in a contiguous
* chunk. The array can be resized. A size increase will cause more memory
* to be allocated, and may result in relocation of the array memory.
* A size decrease has no effect on the memory allocation.
*
* Array elements with constructors or destructors are not supported!
* But if you must have such elements, here's what to know/do:
* - Bitwise copy is used when copying whole arrays.
* - When copying individual items (via push(), insert() etc) the `='
* (equals) operator is used. Thus you should define this operator to do
* a bitwise copy. You should probably also define the copy constructor.
*/
#ifndef _ODE_ARRAY_H_
#define _ODE_ARRAY_H_
#include <ode/odeconfig.h>
// this base class has no constructors or destructor, for your convenience.
class dArrayBase {
protected:
int _size; // number of elements in `data'
int _anum; // allocated number of elements in `data'
void *_data; // array data
void _freeAll (int sizeofT);
void _setSize (int newsize, int sizeofT);
// set the array size to `newsize', allocating more memory if necessary.
// if newsize>_anum and is a power of two then this is guaranteed to
// set _size and _anum to newsize.
public:
// not: dArrayBase () { _size=0; _anum=0; _data=0; }
int size() const { return _size; }
int allocatedSize() const { return _anum; }
void * operator new (size_t size);
void operator delete (void *ptr, size_t size);
void constructor() { _size=0; _anum=0; _data=0; }
// if this structure is allocated with malloc() instead of new, you can
// call this to set it up.
void constructLocalArray (int __anum);
// this helper function allows non-reallocating arrays to be constructed
// on the stack (or in the heap if necessary). this is something of a
// kludge and should be used with extreme care. this function acts like
// a constructor - it is called on uninitialized memory that will hold the
// Array structure and the data. __anum is the number of elements that
// are allocated. the memory MUST be allocated with size:
// sizeof(ArrayBase) + __anum*sizeof(T)
// arrays allocated this way will never try to reallocate or free the
// memory - that's your job.
};
template <class T> class dArray : public dArrayBase {
public:
void equals (const dArray<T> &x) {
setSize (x.size());
memcpy (_data,x._data,x._size * sizeof(T));
}
dArray () { constructor(); }
dArray (const dArray<T> &x) { constructor(); equals (x); }
~dArray () { _freeAll(sizeof(T)); }
void setSize (int newsize) { _setSize (newsize,sizeof(T)); }
T *data() const { return (T*) _data; }
T & operator[] (int i) const { return ((T*)_data)[i]; }
void operator = (const dArray<T> &x) { equals (x); }
void push (const T item) {
if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T));
}
void swap (dArray<T> &x) {
int tmp1;
void *tmp2;
tmp1=_size; _size=x._size; x._size=tmp1;
tmp1=_anum; _anum=x._anum; x._anum=tmp1;
tmp2=_data; _data=x._data; x._data=tmp2;
}
// insert the item at the position `i'. if i<0 then add the item to the
// start, if i >= size then add the item to the end of the array.
void insert (int i, const T item) {
if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
if (i >= (_size-1)) i = _size-1; // add to end
else {
if (i < 0) i=0; // add to start
int n = _size-1-i;
if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T));
}
((T*)_data)[i] = item;
}
void remove (int i) {
if (i >= 0 && i < _size) { // passing this test guarantees size>0
int n = _size-1-i;
if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T));
_size--;
}
}
};
#endif
pyrobolearn/simulators/raisim_wrapper/extras/collision_kernel.h
-293
View File
@@ -1,293 +0,0 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
internal data structures and functions for collision detection.
*/
#ifndef _ODE_COLLISION_KERNEL_H_
#define _ODE_COLLISION_KERNEL_H_
#include <ode/common.h>
#include <ode/contact.h>
#include <ode/collision.h>
#include "objects.h"
#include "odetls.h"
#include "common.h"
//****************************************************************************
// constants and macros
// mask for the number-of-contacts field in the dCollide() flags parameter
#define NUMC_MASK (0xffff)
#define IS_SPACE(geom) \
dIN_RANGE((geom)->type, dFirstSpaceClass, dLastSpaceClass + 1)
#define CHECK_NOT_LOCKED(space) \
dUASSERT ((space) == NULL || (space)->lock_count == 0, \
"Invalid operation for locked space")
//****************************************************************************
// geometry object base class
// geom flags.
//
// GEOM_DIRTY means that the space data structures for this geom are
// potentially not up to date. NOTE THAT all space parents of a dirty geom
// are themselves dirty. this is an invariant that must be enforced.
//
// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date.
// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might
// recalculate its own AABB but does not know how to update the space data
// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY.
// the valid combinations are:
// 0
// GEOM_DIRTY
// GEOM_DIRTY|GEOM_AABB_BAD
// GEOM_DIRTY|GEOM_AABB_BAD|GEOM_POSR_BAD
enum {
GEOM_DIRTY = 1, // geom is 'dirty', i.e. position unknown
GEOM_POSR_BAD = 2, // geom's final posr is not valid
GEOM_AABB_BAD = 4, // geom's AABB is not valid
GEOM_PLACEABLE = 8, // geom is placeable
GEOM_ENABLED = 16, // geom is enabled
GEOM_ZERO_SIZED = 32, // geom is zero sized
GEOM_ENABLE_TEST_MASK = GEOM_ENABLED | GEOM_ZERO_SIZED,
GEOM_ENABLE_TEST_VALUE = GEOM_ENABLED,
// Ray specific
RAY_FIRSTCONTACT = 0x10000,
RAY_BACKFACECULL = 0x20000,
RAY_CLOSEST_HIT = 0x40000
};
enum dxContactMergeOptions {
DONT_MERGE_CONTACTS,
MERGE_CONTACT_NORMALS,
MERGE_CONTACTS_FULLY
};
// geometry object base class. pos and R will either point to a separately
// allocated buffer (if body is 0 - pos points to the dxPosR object) or to
// the pos and R of the body (if body nonzero).
// a dGeomID is a pointer to this object.
struct dxGeom : public dBase {
int type; // geom type number, set by subclass constructor
int gflags; // flags used by geom and space
void *data; // user-defined data pointer
dBodyID body; // dynamics body associated with this object (if any)
dxGeom *body_next; // next geom in body's linked list of associated geoms
dxPosR *final_posr; // final position of the geom in world coordinates
dxPosR *offset_posr; // offset from body in local coordinates
// information used by spaces
dxGeom *next; // next geom in linked list of geoms
dxGeom **tome; // linked list backpointer
dxGeom *next_ex; // next geom in extra linked list of geoms (for higher level structures)
dxGeom **tome_ex; // extra linked list backpointer (for higher level structures)
dxSpace *parent_space;// the space this geom is contained in, 0 if none
dReal aabb[6]; // cached AABB for this space
unsigned long category_bits,collide_bits;
dxGeom (dSpaceID _space, int is_placeable);
virtual ~dxGeom();
// Set or clear GEOM_ZERO_SIZED flag
void updateZeroSizedFlag(bool is_zero_sized) { gflags = is_zero_sized ? (gflags | GEOM_ZERO_SIZED) : (gflags & ~GEOM_ZERO_SIZED); }
// Get parent space TLS kind
unsigned getParentSpaceTLSKind() const;
const dVector3 &buildUpdatedPosition()
{
dIASSERT(gflags & GEOM_PLACEABLE);
recomputePosr();
return final_posr->pos;
}
const dMatrix3 &buildUpdatedRotation()
{
dIASSERT(gflags & GEOM_PLACEABLE);
recomputePosr();
return final_posr->R;
}
// recalculate our new final position if needed
void recomputePosr()
{
if (gflags & GEOM_POSR_BAD) {
computePosr();
gflags &= ~GEOM_POSR_BAD;
}
}
// calculate our new final position from our offset and body
void computePosr();
bool checkControlValueSizeValidity(void *dataValue, int *dataSize, int iRequiresSize) { return (*dataSize == iRequiresSize && dataValue != 0) ? true : !(*dataSize = iRequiresSize); } // Here it is the intent to return true for 0 required size in any case
virtual bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
virtual void computeAABB()=0;
// compute the AABB for this object and put it in aabb. this function
// always performs a fresh computation, it does not inspect the
// GEOM_AABB_BAD flag.
virtual int AABBTest (dxGeom *o, dReal aabb[6]);
// test whether the given AABB object intersects with this object, return
// 1=yes, 0=no. this is used as an early-exit test in the space collision
// functions. the default implementation returns 1, which is the correct
// behavior if no more detailed implementation can be provided.
// utility functions
// compute the AABB only if it is not current. this function manipulates
// the GEOM_AABB_BAD flag.
void recomputeAABB() {
if (gflags & GEOM_AABB_BAD) {
// our aabb functions assume final_posr is up to date
recomputePosr();
computeAABB();
gflags &= ~GEOM_AABB_BAD;
}
}
inline void markAABBBad();
// add and remove this geom from a linked list maintained by a space.
void spaceAdd (dxGeom **first_ptr) {
next = *first_ptr;
tome = first_ptr;
if (*first_ptr) (*first_ptr)->tome = &next;
*first_ptr = this;
}
void spaceRemove() {
if (next) next->tome = tome;
*tome = next;
}
// add and remove this geom from a linked list maintained by a body.
void bodyAdd (dxBody *b) {
body = b;
body_next = b->geom;
b->geom = this;
}
void bodyRemove();
};
//****************************************************************************
// the base space class
//
// the contained geoms are divided into two kinds: clean and dirty.
// the clean geoms have not moved since they were put in the list,
// and their AABBs are valid. the dirty geoms have changed position, and
// their AABBs are may not be valid. the two types are distinguished by the
// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list.
#if dTLS_ENABLED
#define dSPACE_TLS_KIND_INIT_VALUE OTK__DEFAULT
#define dSPACE_TLS_KIND_MANUAL_VALUE OTK_MANUALCLEANUP
#else
#define dSPACE_TLS_KIND_INIT_VALUE 0
#define dSPACE_TLS_KIND_MANUAL_VALUE 0
#endif
struct dxSpace : public dxGeom {
int count; // number of geoms in this space
dxGeom *first; // first geom in list
int cleanup; // cleanup mode, 1=destroy geoms on exit
int sublevel; // space sublevel (used in dSpaceCollide2). NOT TRACKED AUTOMATICALLY!!!
unsigned tls_kind; // space TLS kind to be used for global caches retrieval
// cached state for getGeom()
int current_index; // only valid if current_geom != 0
dxGeom *current_geom; // if 0 then there is no information
// locking stuff. the space is locked when it is currently traversing its
// internal data structures, e.g. in collide() and collide2(). operations
// that modify the contents of the space are not permitted when the space
// is locked.
int lock_count;
dxSpace (dSpaceID _space);
~dxSpace();
void computeAABB();
void setCleanup (int mode) { cleanup = (mode != 0); }
int getCleanup() const { return cleanup; }
void setSublevel(int value) { sublevel = value; }
int getSublevel() const { return sublevel; }
void setManulCleanup(int value) { tls_kind = (value ? dSPACE_TLS_KIND_MANUAL_VALUE : dSPACE_TLS_KIND_INIT_VALUE); }
int getManualCleanup() const { return (tls_kind == dSPACE_TLS_KIND_MANUAL_VALUE) ? 1 : 0; }
int query (dxGeom *geom) const { dAASSERT(geom); return (geom->parent_space == this); }
int getNumGeoms() const { return count; }
virtual dxGeom *getGeom (int i);
virtual void add (dxGeom *);
virtual void remove (dxGeom *);
virtual void dirty (dxGeom *);
virtual void cleanGeoms()=0;
// turn all dirty geoms into clean geoms by computing their AABBs and any
// other space data structures that are required. this should clear the
// GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms.
virtual void collide (void *data, dNearCallback *callback)=0;
virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0;
};
//////////////////////////////////////////////////////////////////////////
/*inline */
void dxGeom::markAABBBad() {
gflags |= (GEOM_DIRTY | GEOM_AABB_BAD);
CHECK_NOT_LOCKED(parent_space);
}
//****************************************************************************
// Initialization and finalization functions
void dInitColliders();
void dFinitColliders();
void dClearPosrCache(void);
void dFinitUserClasses();
#endif
pyrobolearn/simulators/raisim_wrapper/extras/common.h
-239
View File
@@ -1,239 +0,0 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#ifndef _ODE_PRIVATE_COMMON_H_
#define _ODE_PRIVATE_COMMON_H_
#include "typedefs.h"
#include <algorithm>
#ifndef SIZE_MAX
#define SIZE_MAX ((size_t)(-1))
#endif
#ifndef offsetof
#define offsetof(s, m) ((size_t)&(((s *)8)->m) - (size_t)8)
#endif
#ifndef membersize
#define membersize(s, m) (sizeof(((s *)8)->m))
#endif
#ifndef endoffsetof
#define endoffsetof(s, m) ((size_t)((size_t)&(((s *)8)->m) - (size_t)8) + sizeof(((s *)8)->m))
#endif
#define dMACRO_MAX(a, b) ((a) > (b) ? (a) : (b))
#define dMACRO_MIN(a, b) ((a) < (b) ? (a) : (b))
#define dMAKE_PADDING_SIZE(DataType, ElementType) ((sizeof(DataType) + sizeof(ElementType) - 1) / sizeof(ElementType))
/* the efficient alignment. most platforms align data structures to some
* number of bytes, but this is not always the most efficient alignment.
* for example, many x86 compilers align to 4 bytes, but on a pentium it
* is important to align doubles to 8 byte boundaries (for speed), and
* the 4 floats in a SIMD register to 16 byte boundaries. many other
* platforms have similar behavior. setting a larger alignment can waste
* a (very) small amount of memory. NOTE: this number must be a power of
* two. this is set to 16 by default.
*/
#ifndef EFFICIENT_ALIGNMENT
#define EFFICIENT_ALIGNMENT 16
#endif
/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */
#define dEFFICIENT_SIZE(x) (((x) + (EFFICIENT_ALIGNMENT - 1)) & (int)(~(EFFICIENT_ALIGNMENT - 1))) // Casting the mask to int ensures sign-extension to larger integer sizes
#define dEFFICIENT_PTR(p) ((void *)dEFFICIENT_SIZE((uintptr_t)(p)))
#define dOFFSET_EFFICIENTLY(p, b) ((void *)((uintptr_t)(p) + dEFFICIENT_SIZE(b)))
#define dOVERALIGNED_SIZE(size, alignment) dEFFICIENT_SIZE((size) + ((alignment) - EFFICIENT_ALIGNMENT))
#define dOVERALIGNED_PTR(buf_ptr, alignment) ((void *)(((uintptr_t)(buf_ptr) + ((alignment) - 1)) & (int)(~(alignment - 1)))) // Casting the mask to int ensures sign-extension to larger integer sizes
#define dOFFSET_OVERALIGNEDLY(buf_ptr, size, alignment) ((void *)((uintptr_t)(buf_ptr) + dOVERALIGNED_SIZE(size, alignment)))
/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
* up to 15 bytes per allocation, depending on what alloca() returns.
*/
#define dALLOCA16(n) \
dEFFICIENT_PTR(alloca((n)+(EFFICIENT_ALIGNMENT)))
template<typename DstType, typename SrcType>
inline
bool _cast_to_smaller(DstType &dtOutResult, const SrcType &stArgument)
{
return (SrcType)(dtOutResult = (DstType)stArgument) == stArgument;
}
#if defined(__GNUC__)
#define dCAST_TO_SMALLER(TargetType, SourceValue) ({ TargetType ttCastSmallerValue; dIVERIFY(_cast_to_smaller(ttCastSmallerValue, SourceValue)); ttCastSmallerValue; })
#else // #if !defined(__GNUC__)
#define dCAST_TO_SMALLER(TargetType, SourceValue) templateCAST_TO_SMALLER<TargetType>(SourceValue)
template <typename TTargetType, typename TSourceType>
inline TTargetType templateCAST_TO_SMALLER(const TSourceType &stSourceValue)
{
TTargetType ttCastSmallerValue;
dIVERIFY(_cast_to_smaller(ttCastSmallerValue, stSourceValue));
return ttCastSmallerValue;
}
#endif // #if !defined(__GNUC__)
template<typename value_type>
inline
void dxSwap(value_type &one, value_type &another)
{
std::swap(one, another);
}
template<typename value_type, typename lo_type, typename hi_type>
inline
value_type dxClamp(const value_type &value, const lo_type &lo, const hi_type &hi)
{
return value < lo ? (value_type)lo : value > hi ? (value_type)hi : value;
}
template <typename Type>
union _const_type_cast_union
{
explicit _const_type_cast_union(const void *psvCharBuffer): m_psvCharBuffer(psvCharBuffer) {}
operator const Type *() const { return m_pstTypedPointer; }
const Type &operator *() const { return *m_pstTypedPointer; }
const Type *operator ->() const { return m_pstTypedPointer; }
const Type &operator [](ptrdiff_t diElementIndex) const { return m_pstTypedPointer[diElementIndex]; }
const Type &operator [](size_t siElementIndex) const { return m_pstTypedPointer[siElementIndex]; }
const void *m_psvCharBuffer;
const Type *m_pstTypedPointer;
};
template <typename Type>
union _type_cast_union
{
explicit _type_cast_union(void *psvCharBuffer): m_psvCharBuffer(psvCharBuffer) {}
operator Type *() const { return m_pstTypedPointer; }
Type &operator *() const { return *m_pstTypedPointer; }
Type *operator ->() const { return m_pstTypedPointer; }
Type &operator [](ptrdiff_t diElementIndex) const { return m_pstTypedPointer[diElementIndex]; }
Type &operator [](size_t siElementIndex) const { return m_pstTypedPointer[siElementIndex]; }
void *m_psvCharBuffer;
Type *m_pstTypedPointer;
};
template<size_t tsiTypeSize>
struct _sized_signed;
template<>
struct _sized_signed<sizeof(uint8)>
{
typedef int8 type;
};
template<>
struct _sized_signed<sizeof(uint16)>
{
typedef int16 type;
};
template<>
struct _sized_signed<sizeof(uint32)>
{
typedef int32 type;
};
template<>
struct _sized_signed<sizeof(uint64)>
{
typedef int64 type;
};
template<typename tintergraltype>
struct _make_signed
{
typedef typename _sized_signed<sizeof(tintergraltype)>::type type;
};
template<size_t tsiTypeSize>
struct _sized_unsigned;
template<>
struct _sized_unsigned<sizeof(int8)>
{
typedef uint8 type;
};
template<>
struct _sized_unsigned<sizeof(int16)>
{
typedef uint16 type;
};
template<>
struct _sized_unsigned<sizeof(int32)>
{
typedef uint32 type;
};
template<>
struct _sized_unsigned<sizeof(int64)>
{
typedef uint64 type;
};
template<typename tintergraltype>
struct _make_unsigned
{
typedef typename _sized_unsigned<sizeof(tintergraltype)>::type type;
};
// template<typename tvalueint, typename tminint, typename tmaxint>
// inline
// bool dxInRange(tvalueint viValue, tminint miMin, tmaxint miMax)
// {
// return (typename _sized_unsigned<dMACRO_MAX(sizeof(tvalueint), sizeof(tminint))>::type)(viValue - miMin) < (typename _sized_unsigned<dMACRO_MAX(sizeof(tmaxint), sizeof(tminint))>::type)(miMax - miMin);
// }
// #define dIN_RANGE(aval, amin, amax) dxInRange(aval, amin, amax)
#define dIN_RANGE(aval, amin, amax) ((_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)((_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(aval) - (_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(amin)) < (_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)((_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amax) - (_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amin)))
#define dTMPL_IN_RANGE(aval, amin, amax) ((typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)((typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(aval) - (typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(amin)) < (typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)((typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amax) - (typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amin)))
#define dCLAMP(aval, alo, ahi) dxClamp(aval, alo, ahi)
#define dARRAY_SIZE(aarr) (sizeof(aarr) / sizeof((aarr)[0]))
#define dSTATIC_ARRAY_SIZE(aclass, aarr) dARRAY_SIZE(((aclass *)sizeof(void *))->aarr)
#endif
pyrobolearn/simulators/raisim_wrapper/extras/error.h
-100
View File
@@ -1,100 +0,0 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/* Library private error handling functions and macros */
#ifndef _ODE__PRIVATE_ERROR_H_
#define _ODE__PRIVATE_ERROR_H_
#include <ode/error.h>
/* debugging:
* IASSERT is an internal assertion, i.e. a consistency check. if it fails
* we want to know where.
* UASSERT is a user assertion, i.e. if it fails a nice error message
* should be printed for the user.
* AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)"
* is printed.
* DEBUGMSG just prints out a message
*/
# if defined(__STDC__) && __STDC_VERSION__ >= 199901L
# define __FUNCTION__ __func__
# endif
#ifndef dNODEBUG
# ifdef __GNUC__
# define dIASSERT(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
"assertion \"" #a "\" failed in %s() [%s:%u]",__FUNCTION__,__FILE__,__LINE__); } }
# define dUASSERT(a,msg) { if (!(a)) { dDebug (d_ERR_UASSERT, \
msg " in %s()", __FUNCTION__); } }
# define dDEBUGMSG(msg) { dMessage (d_ERR_UASSERT, \
msg " in %s() [%s:%u]", __FUNCTION__,__FILE__,__LINE__); }
# else // not __GNUC__
# define dIASSERT(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
"assertion \"" #a "\" failed in %s:%u",__FILE__,__LINE__); } }
# define dUASSERT(a,msg) { if (!(a)) { dDebug (d_ERR_UASSERT, \
msg " (%s:%u)", __FILE__,__LINE__); } }
# define dDEBUGMSG(msg) { dMessage (d_ERR_UASSERT, \
msg " (%s:%u)", __FILE__,__LINE__); }
# endif
# define dIVERIFY(a) dIASSERT(a)
# define dUVERIFY(a, msg) dUASSERT(a, msg)
#else
# define dIASSERT(a) ((void)0)
# define dUASSERT(a,msg) ((void)0)
# define dDEBUGMSG(msg) ((void)0)
# define dIVERIFY(a) ((void)(a))
# define dUVERIFY(a, msg) ((void)(a))
#endif
#ifdef __GNUC__
#define dUNUSED(Name) Name __attribute__((unused))
#else // not __GNUC__
#define dUNUSED(Name) Name
#endif
#if __cplusplus >= 201103L
#define dSASSERT(e) static_assert(e, #e)
#define dSMSGASSERT(e, message) static_assert(e, message)
#else
#define d_SASSERT_INNER_TOKENPASTE(x, y) x ## y
#define d_SASSERT_TOKENPASTE(x, y) d_SASSERT_INNER_TOKENPASTE(x, y)
#define dSASSERT(e) typedef char dUNUSED(d_SASSERT_TOKENPASTE(d_StaticAssertionFailed_, __LINE__)[(e)?1:-1])
#define dSMSGASSERT(e, message) dSASSERT(e)
#endif
# ifdef __GNUC__
# define dICHECK(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
"assertion \"" #a "\" failed in %s() [%s:%u]",__FUNCTION__,__FILE__,__LINE__); *(int *)0 = 0; } }
# else // not __GNUC__
# define dICHECK(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
"assertion \"" #a "\" failed in %s:%u",__FILE__,__LINE__); *(int *)0 = 0; } }
# endif
// Argument assert is a special case of user assert
#define dAASSERT(a) dUASSERT(a, "Bad argument(s)")
#define dAVERIFY(a) dUVERIFY(a, "Bad argument(s)")
#endif
pyrobolearn/simulators/raisim_wrapper/extras/objects.h
-196
View File
@@ -1,196 +0,0 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
// object, body, and world structs.
#ifndef _ODE__PRIVATE_OBJECTS_H_
#define _ODE__PRIVATE_OBJECTS_H_
#include <ode/common.h>
#include <ode/memory.h>
#include <ode/mass.h>
#include "error.h"
#include "array.h"
#include "threading_base.h"
class dxJointNode; // added by Brian
class dxStepWorkingMemory;
class dxWorldProcessContext;
// some body flags
enum {
dxBodyFlagFiniteRotation = 1, // use finite rotations
dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis
dxBodyDisabled = 4, // body is disabled
dxBodyNoGravity = 8, // body is not influenced by gravity
dxBodyAutoDisable = 16, // enable auto-disable on body
dxBodyLinearDamping = 32, // use linear damping
dxBodyAngularDamping = 64, // use angular damping
dxBodyMaxAngularSpeed = 128,// use maximum angular speed
dxBodyGyroscopic = 256 // use gyroscopic term
};
// base class that does correct object allocation / deallocation
struct dBase {
void *operator new (size_t size) { return dAlloc (size); }
void *operator new (size_t, void *p) { return p; }
void operator delete (void *ptr, size_t size) { dFree (ptr,size); }
void *operator new[] (size_t size) { return dAlloc (size); }
void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); }
};
// base class for bodies and joints
struct dObject : public dBase {
dxWorld *world; // world this object is in
dObject *next; // next object of this type in list
dObject **tome; // pointer to previous object's next ptr
int tag; // used by dynamics algorithms
void *userdata; // user settable data
explicit dObject(dxWorld *w): world(w), next(NULL), tome(NULL), tag(0), userdata(NULL) {}
virtual ~dObject();
};
// auto disable parameters
struct dxAutoDisable {
dReal idle_time; // time the body needs to be idle to auto-disable it
int idle_steps; // steps the body needs to be idle to auto-disable it
unsigned int average_samples; // size of the average_lvel and average_avel buffers
dReal linear_average_threshold; // linear (squared) average velocity threshold
dReal angular_average_threshold; // angular (squared) average velocity threshold
dxAutoDisable() {}
explicit dxAutoDisable(void *);
};
// damping parameters
struct dxDampingParameters {
dReal linear_scale; // multiply the linear velocity by (1 - scale)
dReal angular_scale; // multiply the angular velocity by (1 - scale)
dReal linear_threshold; // linear (squared) average speed threshold
dReal angular_threshold; // angular (squared) average speed threshold
dxDampingParameters() {}
explicit dxDampingParameters(void *);
};
// quick-step parameters
struct dxQuickStepParameters {
int num_iterations; // number of SOR iterations to perform
dReal w; // the SOR over-relaxation parameter
dxQuickStepParameters() {}
explicit dxQuickStepParameters(void *);
};
// contact generation parameters
struct dxContactParameters {
dReal max_vel; // maximum correcting velocity
dReal min_depth; // thickness of 'surface layer'
dxContactParameters() {}
explicit dxContactParameters(void *);
};
// position vector and rotation matrix for geometry objects that are not
// connected to bodies.
struct dxPosR {
dVector3 pos;
dMatrix3 R;
};
struct dxBody : public dObject {
dxJointNode *firstjoint; // list of attached joints
unsigned flags; // some dxBodyFlagXXX flags
dGeomID geom; // first collision geom associated with body
dMass mass; // mass parameters about POR
dMatrix3 invI; // inverse of mass.I
dReal invMass; // 1 / mass.mass
dxPosR posr; // position and orientation of point of reference
dQuaternion q; // orientation quaternion
dVector3 lvel,avel; // linear and angular velocity of POR
dVector3 facc,tacc; // force and torque accumulators
dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none
// auto-disable information
dxAutoDisable adis; // auto-disable parameters
dReal adis_timeleft; // time left to be idle
int adis_stepsleft; // steps left to be idle
dVector3* average_lvel_buffer; // buffer for the linear average velocity calculation
dVector3* average_avel_buffer; // buffer for the angular average velocity calculation
unsigned int average_counter; // counter/index to fill the average-buffers
int average_ready; // indicates ( with = 1 ), if the Body's buffers are ready for average-calculations
void (*moved_callback)(dxBody*); // let the user know the body moved
dxDampingParameters dampingp; // damping parameters, depends on flags
dReal max_angular_speed; // limit the angular velocity to this magnitude
dxBody(dxWorld *w);
};
struct dxWorld : public dBase, public dxThreadingBase, private dxIThreadingDefaultImplProvider {
dxBody *firstbody; // body linked list
dxJoint *firstjoint; // joint linked list
int nb,nj; // number of bodies and joints in lists
dVector3 gravity; // gravity vector (m/s/s)
dReal global_erp; // global error reduction parameter
dReal global_cfm; // global constraint force mixing parameter
dxAutoDisable adis; // auto-disable parameters
int body_flags; // flags for new bodies
unsigned islands_max_threads; // maximum threads to allocate for island processing
dxStepWorkingMemory *wmem; // Working memory object for dWorldStep/dWorldQuickStep
dxQuickStepParameters qs;
dxContactParameters contactp;
dxDampingParameters dampingp; // damping parameters
dReal max_angular_speed; // limit the angular velocity to this magnitude
void* userdata;
dxWorld();
virtual ~dxWorld(); // Compilers emit warnings if a class with virtual methods does not have a virtual destructor :(
static bool InitializeDefaultThreading();
static void FinalizeDefaultThreading();
void AssignThreadingImpl(const dxThreadingFunctionsInfo *functions_info, dThreadingImplementationID threading_impl);
unsigned GetThreadingIslandsMaxThreadsCount(unsigned *out_active_thread_count_ptr=NULL) const;
dxWorldProcessContext *UnsafeGetWorldProcessingContext() const;
private: // dxIThreadingDefaultImplProvider
virtual const dxThreadingFunctionsInfo *RetrieveThreadingDefaultImpl(dThreadingImplementationID &out_default_impl);
};
#endif // #ifndef _ODE__PRIVATE_OBJECTS_H_
pyrobolearn/simulators/raisim_wrapper/extras/odeou.h
-102
View File
@@ -1,102 +0,0 @@
/*************************************************************************
* *
* OU library interface file for Open Dynamics Engine, *
* Copyright (C) 2008 Oleh Derevenko. All rights reserved. *
* Email: odar@eleks.com (change all "a" to "e") *
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
ODE interface to OU library functions.
*/
#ifndef _ODE_ODEOU_H_
#define _ODE_ODEOU_H_
#if dOU_ENABLED
#include <ou/assert.h>
#include <ou/enumarrays.h>
#include <ou/macros.h>
#include <ou/templates.h>
#include <ou/typewrapper.h>
#include <ou/simpleflags.h>
#include <ou/customization.h>
#if dATOMICS_ENABLED
#include <ou/atomic.h>
#include <ou/atomicflags.h>
#endif
#if dTLS_ENABLED
#include <ou/threadlocalstorage.h>
#endif
using _OU_NAMESPACE::CEnumUnsortedElementArray;
using _OU_NAMESPACE::CEnumSortedElementArray;
#if dATOMICS_ENABLED
using _OU_NAMESPACE::atomicord32;
using _OU_NAMESPACE::atomicptr;
using _OU_NAMESPACE::InitializeAtomicAPI;
using _OU_NAMESPACE::FinalizeAtomicAPI;
using _OU_NAMESPACE::AtomicCompareExchange;
using _OU_NAMESPACE::AtomicExchange;
using _OU_NAMESPACE::AtomicExchangeAddNoResult;
using _OU_NAMESPACE::AtomicExchangeAdd;
using _OU_NAMESPACE::AtomicCompareExchangePointer;
using _OU_NAMESPACE::AtomicExchangePointer;
#endif
class COdeOu
{
public:
static bool DoOUCustomizations();
static void UndoOUCustomizations();
#if dATOMICS_ENABLED
static bool InitializeAtomics() { return InitializeAtomicAPI(); }
static void FinalizeAtomics() { FinalizeAtomicAPI(); }
#endif
};
#endif
#if !dOU_ENABLED || !dATOMICS_ENABLED
typedef unsigned int atomicord32;
typedef void *atomicptr;
#endif // dOU_ENABLED
#endif // _ODE_ODEOU_H_
pyrobolearn/simulators/raisim_wrapper/extras/odetls.h
-126
View File
@@ -1,126 +0,0 @@
/*************************************************************************
* *
* Thread local storage access stub for Open Dynamics Engine, *
* Copyright (C) 2008 Oleh Derevenko. All rights reserved. *
* Email: odar@eleks.com (change all "a" to "e") *
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
ODE Thread Local Storage access stub interface.
*/
#ifndef _ODE_ODETLS_H_
#define _ODE_ODETLS_H_
#include "odeou.h"
#if dTLS_ENABLED
using _OU_NAMESPACE::tlsvaluetype;
using _OU_NAMESPACE::HTLSKEY;
using _OU_NAMESPACE::CThreadLocalStorage;
struct TrimeshCollidersCache;
enum EODETLSKIND
{
OTK__MIN,
OTK_AUTOCLEANUP = OTK__MIN,
OTK_MANUALCLEANUP,
OTK__MAX,
OTK__DEFAULT = OTK_AUTOCLEANUP,
};
enum EODETLSITEM
{
OTI_DATA_ALLOCATION_FLAGS,
OTI_TRIMESH_TRIMESH_COLLIDER_CACHE,
OTI__MAX,
};
class COdeTls
{
public:
static bool Initialize(EODETLSKIND tkTLSKind);
static void Finalize(EODETLSKIND tkTLSKind);
static void CleanupForThread();
public:
static unsigned GetDataAllocationFlags(EODETLSKIND tkTLSKind)
{
// Must be a safe call as it is used to test if TLS slot is allocated at all
return (unsigned)(size_t)CThreadLocalStorage::GetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS);
}
static void SignalDataAllocationFlags(EODETLSKIND tkTLSKind, unsigned uFlagsMask)
{
unsigned uCurrentFlags = (unsigned)(size_t)CThreadLocalStorage::UnsafeGetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS);
CThreadLocalStorage::UnsafeSetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS, (tlsvaluetype)(size_t)(uCurrentFlags | uFlagsMask));
}
static void DropDataAllocationFlags(EODETLSKIND tkTLSKind, unsigned uFlagsMask)
{
unsigned uCurrentFlags = (unsigned)(size_t)CThreadLocalStorage::UnsafeGetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS);
CThreadLocalStorage::UnsafeSetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_DATA_ALLOCATION_FLAGS, (tlsvaluetype)(size_t)(uCurrentFlags & ~uFlagsMask));
}
static TrimeshCollidersCache *GetTrimeshCollidersCache(EODETLSKIND tkTLSKind)
{
return (TrimeshCollidersCache *)CThreadLocalStorage::UnsafeGetStorageValue(m_ahtkStorageKeys[tkTLSKind], OTI_TRIMESH_TRIMESH_COLLIDER_CACHE);
}
public:
static bool AssignDataAllocationFlags(EODETLSKIND tkTLSKind, unsigned uInitializationFlags);
static bool AssignTrimeshCollidersCache(EODETLSKIND tkTLSKind, TrimeshCollidersCache *pccInstance);
static void DestroyTrimeshCollidersCache(EODETLSKIND tkTLSKind);
private:
static void FreeTrimeshCollidersCache(TrimeshCollidersCache *pccCacheInstance);
private:
static void _OU_CONVENTION_CALLBACK FreeTrimeshCollidersCache_Callback(tlsvaluetype vValueData);
private:
static HTLSKEY m_ahtkStorageKeys[OTK__MAX];
};
#endif // dTLS_ENABLED
#endif // _ODE_ODETLS_H_
pyrobolearn/simulators/raisim_wrapper/extras/threading_base.h
-251
View File
@@ -1,251 +0,0 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* Threading base wrapper class header file. *
* Copyright (C) 2011-2012 Oleh Derevenko. All rights reserved. *
* e-mail: odar@eleks.com (change all "a" to "e") *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
* Threading base class to be used for inheritance by dxWorld, dxSpace and others
* to take advantage of threaded execution.
*/
#ifndef _ODE_THREADING_BASE_H_
#define _ODE_THREADING_BASE_H_
#include <ode/threading.h>
struct dxThreadingBase;
struct dxIThreadingDefaultImplProvider
{
public:
virtual const dxThreadingFunctionsInfo *RetrieveThreadingDefaultImpl(dThreadingImplementationID &out_default_impl) = 0;
};
struct dxThreadingBase
{
protected:
dxThreadingBase():
m_default_impl_provider(NULL),
m_functions_info(NULL),
m_threading_impl(NULL)
{
}
// This ought to be done via constructor, but passing 'this' in base class initializer emits a warning in MSVC :(
void SetThreadingDefaultImplProvider(dxIThreadingDefaultImplProvider *default_impl_provider) { m_default_impl_provider = default_impl_provider; }
public:
void AssignThreadingImpl(const dxThreadingFunctionsInfo *functions_info, dThreadingImplementationID threading_impl)
{
dAASSERT((functions_info == NULL) == (threading_impl == NULL));
m_functions_info = functions_info;
m_threading_impl = threading_impl;
}
public:
dMutexGroupID AllocMutexGroup(dmutexindex_t Mutex_count, const char *const *Mutex_names_ptr/*=NULL*/) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
return functions->alloc_mutex_group(impl, Mutex_count, Mutex_names_ptr);
}
void FreeMutexGroup(dMutexGroupID mutex_group) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->free_mutex_group(impl, mutex_group);
}
void LockMutexGroupMutex(dMutexGroupID mutex_group, dmutexindex_t mutex_index) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->lock_group_mutex(impl, mutex_group, mutex_index);
}
// bool TryLockMutexGroupMutex(dMutexGroupID mutex_group, dmutexindex_t mutex_index) const
// {
// dThreadingImplementationID impl;
// const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
// return functions->trylock_group_mutex(impl, mutex_group, mutex_index) != 0;
// }
void UnlockMutexGroupMutex(dMutexGroupID mutex_group, dmutexindex_t mutex_index) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->unlock_group_mutex(impl, mutex_group, mutex_index);
}
dCallWaitID AllocThreadedCallWait() const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
return functions->alloc_call_wait(impl);
}
void ResetThreadedCallWait(dCallWaitID call_wait) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->reset_call_wait(impl, call_wait);
}
void FreeThreadedCallWait(dCallWaitID call_wait) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->free_call_wait(impl, call_wait);
}
void PostThreadedCall(int *out_summary_fault/*=NULL*/,
dCallReleaseeID *out_post_releasee/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/,
dCallWaitID call_wait/*=NULL*/,
dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index,
const char *call_name/*=NULL*/) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->post_call(impl, out_summary_fault, out_post_releasee, dependencies_count, dependent_releasee, call_wait, call_func, call_context, instance_index, call_name);
}
void AlterThreadedCallDependenciesCount(dCallReleaseeID target_releasee,
ddependencychange_t dependencies_count_change) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->alter_call_dependencies_count(impl, target_releasee, dependencies_count_change);
}
void WaitThreadedCallExclusively(int *out_wait_status/*=NULL*/,
dCallWaitID call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/,
const char *wait_name/*=NULL*/) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->wait_call(impl, out_wait_status, call_wait, timeout_time_ptr, wait_name);
functions->reset_call_wait(impl, call_wait);
}
void WaitThreadedCallCollectively(int *out_wait_status/*=NULL*/,
dCallWaitID call_wait, const dThreadedWaitTime *timeout_time_ptr/*=NULL*/,
const char *wait_name/*=NULL*/) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
functions->wait_call(impl, out_wait_status, call_wait, timeout_time_ptr, wait_name);
}
unsigned RetrieveThreadingThreadCount() const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
return functions->retrieve_thread_count(impl);
}
bool PreallocateResourcesForThreadedCalls(unsigned max_simultaneous_calls_estimate) const
{
dThreadingImplementationID impl;
const dxThreadingFunctionsInfo *functions = FindThreadingImpl(impl);
return functions->preallocate_resources_for_calls(impl, max_simultaneous_calls_estimate) != 0;
}
public:
void PostThreadedCallsGroup(int *out_summary_fault/*=NULL*/,
ddependencycount_t member_count, dCallReleaseeID dependent_releasee/*=NULL*/,
dThreadedCallFunction *call_func, void *call_context,
const char *call_name/*=NULL*/) const;
void PostThreadedCallsIndexOverridenGroup(int *out_summary_fault/*=NULL*/,
ddependencycount_t member_count, dCallReleaseeID dependent_releasee/*=NULL*/,
dThreadedCallFunction *call_func, void *call_context, unsigned index_override,
const char *call_name/*=NULL*/) const;
void PostThreadedCallForUnawareReleasee(int *out_summary_fault/*=NULL*/,
dCallReleaseeID *out_post_releasee/*=NULL*/, ddependencycount_t dependencies_count, dCallReleaseeID dependent_releasee/*=NULL*/,
dCallWaitID call_wait/*=NULL*/,
dThreadedCallFunction *call_func, void *call_context, dcallindex_t instance_index,
const char *call_name/*=NULL*/) const;
protected:
const dxThreadingFunctionsInfo *FindThreadingImpl(dThreadingImplementationID &out_impl_found) const;
private:
const dxThreadingFunctionsInfo *GetFunctionsInfo() const { return m_functions_info; }
dThreadingImplementationID GetThreadingImpl() const { return m_threading_impl; }
private:
dxIThreadingDefaultImplProvider *m_default_impl_provider;
const dxThreadingFunctionsInfo *m_functions_info;
dThreadingImplementationID m_threading_impl;
};
class dxMutexGroupLockHelper
{
public:
dxMutexGroupLockHelper(dxThreadingBase *threading_base, dMutexGroupID mutex_group, dmutexindex_t mutex_index):
m_threading_base(threading_base),
m_mutex_group(mutex_group),
m_mutex_index(mutex_index),
m_mutex_locked(true)
{
threading_base->LockMutexGroupMutex(mutex_group, mutex_index);
}
~dxMutexGroupLockHelper()
{
if (m_mutex_locked)
{
m_threading_base->UnlockMutexGroupMutex(m_mutex_group, m_mutex_index);
}
}
void UnlockMutex()
{
dIASSERT(m_mutex_locked);
m_threading_base->UnlockMutexGroupMutex(m_mutex_group, m_mutex_index);
m_mutex_locked = false;
}
void RelockMutex()
{
dIASSERT(!m_mutex_locked);
m_threading_base->LockMutexGroupMutex(m_mutex_group, m_mutex_index);
m_mutex_locked = true;
}
private:
dxThreadingBase *m_threading_base;
dMutexGroupID m_mutex_group;
dmutexindex_t m_mutex_index;
bool m_mutex_locked;
};
#endif // #ifndef _ODE_THREADING_BASE_H_
pyrobolearn/simulators/raisim_wrapper/extras/typedefs.h
-48
View File
@@ -1,48 +0,0 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* General Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#ifndef _ODE_TYPEDEFS_H_
#define _ODE_TYPEDEFS_H_
#include <ode/odeconfig.h>
/*
* Internal typedefs to map public types into more convenient private types
*/
typedef dint64 int64;
typedef duint64 uint64;
typedef dint32 int32;
typedef duint32 uint32;
typedef dint16 int16;
typedef duint16 uint16;
typedef dint8 int8;
typedef duint8 uint8;
typedef dintptr intptr;
typedef duintptr uintptr;
typedef ddiffint diffint;
typedef dsizeint sizeint;
#endif
pyrobolearn/simulators/raisim_wrapper/include/converter.hpp
-158
View File
@@ -1,158 +0,0 @@
/**
* Type converters used to convert between different data types.
*
* Copyright (c) 2019, Brian Delhaisse <briandelhaisse@gmail.com>
*
* 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 <pybind11/pybind11.h>
#include <pybind11/numpy.h> // numpy types
#include <sstream> // for ostringstream
#include "raisim/math.hpp" // contains the definitions of Vec, Mat, VecDyn, MatDyn, etc.
#include "Eigen/Geometry" // for Eigen::Quaterniond
namespace py = pybind11;
/// \brief: convert from raisim::Vec<n> to np.array
template<size_t n>
py::array_t<double> convert_vec_to_np(raisim::Vec<n> &vec) {
const double *ptr = vec.ptr(); // get data pointer
// return np.array[float64[n]]
return py::array_t<double>(
{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<n>
template<size_t n>
raisim::Vec<n> convert_np_to_vec(py::array_t<double> 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<n> vec;
// copy the data
for(size_t i=0; i<n; i++) {
vec[i] = *array.data(i);
}
// return vector
return vec;
}
/// \brief: convert from raisim::Mat<n,m> to np.array[float64[n,m]]
template<size_t n, size_t m>
py::array_t<double> convert_mat_to_np(const raisim::Mat<n, m> &mat) {
const double *ptr = mat.ptr(); // get data pointer
// return np.array[float64[n,m]]
return py::array_t<double>(
{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<n,m>
template<size_t n, size_t m>
raisim::Mat<n, m> convert_np_to_mat(py::array_t<double> 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<n, m> mat;
// copy the data
for (size_t i=0; i<n; i++)
for (size_t j=0; j<m; j++)
mat[i, j] = *array.data(i, j);
// return matrix
return mat;
}
/// \brief: convert from raisim::VecDyn to np.array[float64[n]]
py::array_t<double> 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<double> array);
/// \brief: convert from raisim::MatDyn to np.array[float64[n,m]]
py::array_t<double> 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<double> array);
/// \brief: convert from raisim::Transformation to np.array[float[4,4]]
py::array_t<double> convert_transformation_to_np(const raisim::Transformation &transfo);
/// \brief: convert from np.array[float[4,4]] to raisim::Transformation
raisim::Transformation convert_np_to_transformation(py::array_t<double> array);
/// \brief: convert from Eigen::Quaterniond to np.array[float[4]]
py::array_t<double> convert_quaternion_to_np(const Eigen::Quaterniond &quaternion);
/// \brief: convert from np.array[float[4]] to Eigen::Quaterniond
Eigen::Quaterniond convert_np_to_quaternion(py::array_t<double> array);
#endif
pyrobolearn/simulators/raisim_wrapper/include/visualizer/LICENSE
-19
View File
@@ -1,19 +0,0 @@
Copyright (c) 2019 Robotic Systems Lab, ETH Zurich
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.
pyrobolearn/simulators/raisim_wrapper/include/visualizer/deserializer.hpp
-344
View File
@@ -1,344 +0,0 @@
//
// Created by Jemin Hwangbo on 3/3/19.
// MIT License
//
// Copyright (c) 2019-2019 Robotic Systems Lab, ETH Zurich
//
// 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 RAISIMOGREVISUALIZER_DESERIALIZER_HPP
#define RAISIMOGREVISUALIZER_DESERIALIZER_HPP
#include "raisim/OgreVis.hpp"
#include "raisim/RaisimServer.hpp"
#include <experimental/filesystem>
using namespace raisim;
namespace raisim {
class Deserializer {
public:
explicit Deserializer(const std::string &resDir) {
resDir_ = resDir;
if(resDir_.back() =='/')
resDir_ = resDir.substr(0, resDir_.size()-1);
receiveVector_.resize(raisim::RaisimServer::SEND_BUFFER_SIZE);
};
void estabilishConnection() {
///
sock_ = 0;
std::vector<char> buffer;
buffer.resize(raisim::RaisimServer::SEND_BUFFER_SIZE);
RSFATAL_IF((sock_ = socket(AF_INET, SOCK_STREAM, 0)) < 0, "Socket creation error");
memset(&serv_addr, '0', sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
serv_addr.sin_port = htons(raisim::RaisimServer::RAISIM_PORT);
// Convert IPv4 and IPv6 addresses from text to binary form
RSFATAL_IF(inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0, "Invalid address");
// RSFATAL_IF(connect(sock_, (sockaddr *) &serv_addr, sizeof(serv_addr)) < 0, "Connection failed");
RSINFO("Waiting for a response from a server")
while(connect(sock_, (sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
}
}
int connectToServer() {
return connect(sock_, (sockaddr *) &serv_addr, sizeof(serv_addr));
}
void closeSocket() {
close(sock_);
}
/// return if reinitialization is necessary
int updatePosition() {
int request = raisim::RaisimServer::ClientMessageType::REQUEST_OBJECT_POSITION;
send(sock_, &request, sizeof(int), 0);
auto status = readData();
if(!status)
return status;
auto vis = OgreVis::get();
char* data = &receiveVector_[0];
int state;
data = RaisimServer::get(data, &state);
if(state == RaisimServer::STATUS_TERMINATING)
return 0;
RaisimServer::ServerMessageType type;
data = RaisimServer::get(data, &type);
if(type == RaisimServer::ServerMessageType::NO_MESSAGE) {
usleep(5e4);
return 1;
}
unsigned long configurationNumber;
data = RaisimServer::get(data, &configurationNumber);
size_t numberOfObjects;
data = RaisimServer::get(data, &numberOfObjects);
for(size_t i=0; i<numberOfObjects; i++) {
// set name length
size_t localIdxSize;
data = RaisimServer::get(data, &localIdxSize);
for(size_t j=0; j < localIdxSize; j++) {
std::string name;
data = RaisimServer::getString(data, name);
double posX, posY, posZ;
double quatW, quatx, quaty, quatz;
data = RaisimServer::get(data, &posX);
data = RaisimServer::get(data, &posY);
data = RaisimServer::get(data, &posZ);
data = RaisimServer::get(data, &quatW);
data = RaisimServer::get(data, &quatx);
data = RaisimServer::get(data, &quaty);
data = RaisimServer::get(data, &quatz);
vis->getVisualObjectList()[name].graphics->setPosition(posX, posY, posZ);
vis->getVisualObjectList()[name].graphics->setOrientation(quatW, quatx, quaty, quatz);
}
}
memset(&receiveVector_[0], 0, receiveVector_.size() * sizeof(receiveVector_[0]));
return status;
}
inline int init() {
int request = raisim::RaisimServer::ClientMessageType::REQUEST_INITIALIZATION;
send(sock_, &request, sizeof(int), 0);
if(!readData())
return 0;
char* data = &receiveVector_[0];
auto vis = OgreVis::get();
vis->clearVisualObject();
int state;
data = RaisimServer::get(data, &state);
if(state == RaisimServer::STATUS_TERMINATING)
return 0;
RaisimServer::ServerMessageType messageType;
data = RaisimServer::get(data, &messageType);
data = RaisimServer::get(data, &configurationNumber_);
size_t size;
auto &vobVec = vis->getVisualObjectList();
data = RaisimServer::get(data, &size);
std::vector<float> heights;
for (size_t i = 0; i < size; i++) {
std::string meshName;
size_t obIndex;
data = RaisimServer::get(data, &obIndex);
raisim::Vec<3> scale, offset = {0, 0, 0};
raisim::Mat<3, 3> rot;
rot.setIdentity();
raisim::ObjectType type;
data = RaisimServer::get(data, &type);
float radius, height, x, y, z;
switch (type) {
case raisim::ObjectType::SPHERE:
data = RaisimServer::get(data, &radius);
scale = {radius, radius, radius};
meshName = "sphereMesh";
vis->addVisualObject(std::to_string(obIndex), meshName, "default", scale);
break;
case raisim::ObjectType::BOX:
data = RaisimServer::get(data, &x);
data = RaisimServer::get(data, &y);
data = RaisimServer::get(data, &z);
scale = {x, y, z};
meshName = "cubeMesh";
vis->addVisualObject(std::to_string(obIndex), meshName, "default", scale);
break;
case raisim::ObjectType::CYLINDER:
data = RaisimServer::get(data, &radius);
data = RaisimServer::get(data, &height);
scale = {radius, radius, height};
meshName = "cylinderMesh";
vis->addVisualObject(std::to_string(obIndex), meshName, "default", scale);
break;
case raisim::ObjectType::CAPSULE:
data = RaisimServer::get(data, &radius);
data = RaisimServer::get(data, &height);
scale = {radius, radius, height};
meshName = "capsuleMesh";
break;
case raisim::ObjectType::HALFSPACE:
data = RaisimServer::get(data, &height);
scale = {20, 20, 1};
offset = {0, 0, height};
meshName = "planeMesh";
vis->addVisualObject(std::to_string(obIndex), meshName, "checkerboard_green", scale, false, 1<<0);
break;
case raisim::ObjectType::HEIGHTMAP:
float centX, centY, sizeX, sizeY;
size_t sampleX, sampleY, heightSize;
data = RaisimServer::get(data, &centX);
data = RaisimServer::get(data, &centY);
data = RaisimServer::get(data, &sizeX);
data = RaisimServer::get(data, &sizeY);
data = RaisimServer::get(data, &sampleX);
data = RaisimServer::get(data, &sampleY);
data = RaisimServer::get(data, &heightSize);
heights.resize(heightSize);
data = RaisimServer::getN(data, heights.data(), heightSize);
vis->buildHeightMap(std::to_string(obIndex), sampleX, sizeX, centX, sampleY, sizeY, centY, heights);
scale = {1, 1, 1};
offset = {0, 0, height};
meshName = std::to_string(obIndex);
vis->addVisualObject(std::to_string(obIndex), meshName, "default", scale);
break;
case raisim::ObjectType::ARTICULATED_SYSTEM:
std::string objResDir, topDir, localObjResDir;
data = RaisimServer::getString(data, objResDir);
topDir = raisim::getFileName(objResDir);
localObjResDir = resDir_ + separator() + topDir;
RSFATAL_IF(!raisim::directoryExists(localObjResDir),
"Required resource directory " + localObjResDir + " is missing")
for (size_t visItem = 0; visItem < 2; visItem++) {
size_t numberOfVisObjects;
data = RaisimServer::get(data, &numberOfVisObjects);
for (size_t j = 0; j < numberOfVisObjects; j++) {
raisim::Shape::Type shape;
data = RaisimServer::get(data, &shape);
unsigned long int group;
data = RaisimServer::get(data, &group);
std::string subName = std::to_string(obIndex) + separator() + std::to_string(visItem) + separator() + std::to_string(j);
if (shape == Shape::Mesh) {
std::string meshFile, fileName;
data = RaisimServer::getString(data, meshFile);
double sx, sy, sz;
data = RaisimServer::get(data, &sx);
data = RaisimServer::get(data, &sy);
data = RaisimServer::get(data, &sz);
scale = {sx, sy, sz};
fileName = raisim::getFileName(meshFile);
vis->loadMeshFile(resDir_ + separator() + topDir + separator() + fileName,
topDir + separator() + fileName);
vis->addVisualObject(subName,
topDir + separator() + fileName,
"default",
scale,
true,
1<<group);
} else {
std::vector<double> visParam;
data = RaisimServer::getStdVector(data, visParam);
switch (shape) {
case Shape::Box:
scale = {visParam[0], visParam[1], visParam[2]};
meshName = "cubeMesh";
break;
case Shape::Capsule:
scale = {visParam[0], visParam[0], visParam[1]};
meshName = "capsuleMesh";
break;
case Shape::Cylinder:
scale = {visParam[0], visParam[0], visParam[1]};
meshName = "cylinderMesh";
break;
case Shape::Sphere:
scale = {visParam[0], visParam[0], visParam[0]};
meshName = "sphereMesh";
break;
}
vis->addVisualObject(subName, meshName, "default", scale, true, 1<<group);
vis->getVisualObjectList()[subName].graphics->setScale(scale[0], scale[1], scale[2]);
}
}
}
break;
}
}
memset(&receiveVector_[0], 0, receiveVector_.size() * sizeof(receiveVector_[0]));
return 1;
}
private:
int readData() {
char *data = &receiveVector_[0];
char footer = 'c';
while (footer == 'c') {
valread = recv(sock_, data, raisim::RaisimServer::MAXIMUM_PACKET_SIZE, 0);
if(valread == 0) break;
footer = data[raisim::RaisimServer::MAXIMUM_PACKET_SIZE - raisim::RaisimServer::FOOTER_SIZE];
data += valread - raisim::RaisimServer::FOOTER_SIZE;
}
return data - &receiveVector_[0];
}
int sock_ = 0, valread;
std::vector<char> receiveVector_;
sockaddr_in serv_addr;
unsigned long configurationNumber_;
std::string resDir_;
};
}
#endif //RAISIMOGREVISUALIZER_DESERIALIZER_HPP
pyrobolearn/simulators/raisim_wrapper/include/visualizer/guiState.hpp
-40
View File
@@ -1,40 +0,0 @@
//
// Created by Jemin Hwangbo on 4/11/19.
// MIT License
//
// Copyright (c) 2019-2019 Robotic Systems Lab, ETH Zurich
//
// 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 RAISIMOGREVISUALIZER_GUISTATE_HPP
#define RAISIMOGREVISUALIZER_GUISTATE_HPP
namespace raisim{
namespace gui {
static bool manualStepping = false;
static bool showBodies = true;
static bool showCollision = false;
static bool showContacts = false;
static bool showForces = false;
}
}
#endif //RAISIMOGREVISUALIZER_GUISTATE_HPP
pyrobolearn/simulators/raisim_wrapper/include/visualizer/helper.hpp
-43
View File
@@ -1,43 +0,0 @@
//
// Created by Jemin Hwangbo on 2/28/19.
// MIT License
//
// Copyright (c) 2019-2019 Robotic Systems Lab, ETH Zurich
//
// 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 RAISIMOGREVISUALIZER_HELPER_HPP
#define RAISIMOGREVISUALIZER_HELPER_HPP
#include <string>
#define MAKE_STR(x) _MAKE_STR(x)
#define _MAKE_STR(x) #x
namespace raisim {
inline std::string loadResource (const std::string& file) {
return std::string(MAKE_STR(EXAMPLE_ROBOT_RESOURCE_DIR))+file;
}
}
#endif //RAISIMOGREVISUALIZER_HELPER_HPP
pyrobolearn/simulators/raisim_wrapper/include/visualizer/raisimBasicImguiPanel.hpp
-307
View File
@@ -1,307 +0,0 @@
//
// Created by Jemin Hwangbo on 2/28/19.
// MIT License
//
// Copyright (c) 2019-2019 Robotic Systems Lab, ETH Zurich
//
// 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 RAISIMOGREVISUALIZER_RAISIMBASICIMGUIPANEL_HPP
#define RAISIMOGREVISUALIZER_RAISIMBASICIMGUIPANEL_HPP
#include "guiState.hpp"
ImFont* fontBig;
ImFont* fontMid;
ImFont* fontSmall;
void imguiRenderCallBack() {
ImGui::SetNextWindowPos({0, 0});
// ImGui::SetNextWindowSize({400, 1000}, 0);
if (!ImGui::Begin("RaiSim Application Window")) {
// Early out if the window is collapsed, as an optimization.
ImGui::End();
return;
}
auto vis = raisim::OgreVis::get();
auto world = vis->getWorld();
vis->getPaused() = raisim::gui::manualStepping;
unsigned long mask = 0;
ImGui::PushFont(fontBig);
ImGui::Text("Visualization");
ImGui::Separator();
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Checkbox("Bodies", &raisim::gui::showBodies);
ImGui::Checkbox("Collision Bodies", &raisim::gui::showCollision);
ImGui::Checkbox("Contact Points", &raisim::gui::showContacts);
ImGui::Checkbox("Contact Forces", &raisim::gui::showForces);
ImGui::PopFont();
if(raisim::gui::showBodies) mask |= raisim::OgreVis::RAISIM_OBJECT_GROUP;
if(raisim::gui::showCollision) mask |= raisim::OgreVis::RAISIM_COLLISION_BODY_GROUP;
if(raisim::gui::showContacts) mask |= raisim::OgreVis::RAISIM_CONTACT_POINT_GROUP;
if(raisim::gui::showForces) mask |= raisim::OgreVis::RAISIM_CONTACT_FORCE_GROUP;
vis->setVisibilityMask(mask);
if(raisim::gui::manualStepping) {
if(vis->getTakeNSteps() == -1)
vis->getTakeNSteps() = 0;
} else {
vis->getTakeNSteps() = -1;
}
if (ImGui::CollapsingHeader("Simulation")) {
ImGui::PushFont(fontMid);
ImGui::Text("Sim time: %8.3f, Time step: %8.3f", world->getWorldTime(), world->getTimeStep());
static int takeNSteps = 1;
ImGui::Checkbox("Manual stepping", &raisim::gui::manualStepping);
if(raisim::gui::manualStepping) {
std::string tempString = "Remaining Steps: " + std::to_string(vis->getTakeNSteps());
ImGui::Text("%s", tempString.c_str());
ImGui::Text("Take "); ImGui::SameLine(); ImGui::InputInt("", &takeNSteps); ImGui::SameLine(); ImGui::Text(" steps"); ImGui::SameLine();
if(ImGui::Button("Run"))
vis->getTakeNSteps() += takeNSteps;
} else {
if(ImGui::Button("Set to real time"))
vis->getRealTimeFactorReference() = 1.f;
ImGui::SameLine();
ImGui::SliderFloat("", &vis->getRealTimeFactorReference(), 1e-3, 1000, "Real time factor %5.4f", 10);
}
ImGui::PopFont();
}
auto selected = vis->getSelected();
auto ro = std::get<0>(selected);
auto li = std::get<1>(selected);
if (ImGui::CollapsingHeader("Object data")) {
if(ro) {
ImGui::PushFont(fontBig);
if(!ro->getName().empty()){
ImGui::Text("%s", ("name: " + ro->getName() + "/" + vis->getSelectedGraphicalObject()->name).c_str());
} else
ImGui::Text("Unnamed object");
raisim::Vec<3> pos; ro->getPosition_W(li, pos);
raisim::Vec<3> vel; ro->getVelocity_W(li, vel);
raisim::Vec<4> ori; raisim::Mat<3,3> mat; ro->getOrientation_W(li, mat); raisim::rotMatToQuat(mat, ori);
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Text("Position");
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Text("x = %2.2f, y = %2.2f, z = %2.2f", pos[0], pos[1], pos[2]);
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Text("Velocity");
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Text("x = %2.2f, y = %2.2f, z = %2.2f", vel[0], vel[1], vel[2]);
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Text("Orientation");
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Text("x = %2.2f, x = %2.2f, y = %2.2f, z = %2.2f", ori[0], ori[1], ori[2], ori[3]);
ImGui::PopFont();
ImGui::PushFont(fontMid);
ImGui::Text("Ncontacts: %lu", ro->getContacts().size());
ImGui::PopFont();
}
}
if (ImGui::CollapsingHeader("Contacts")) {
ImGui::PushFont(fontMid);
ImGui::Text("Solver Iterations: %d", world->getContactSolver().getLoopCounter());
ImGui::Text("Total number of contacts: %lu", world->getContactProblem()->size());
std::vector<float> error;
error.reserve(world->getContactSolver().getLoopCounter());
for(int i=0; i<world->getContactSolver().getLoopCounter(); i++)
error.push_back(float(log(world->getContactSolver().getErrorHistory()[i])));
ImGui::PlotLines("Lines", &error[0], error.size(), 0, "avg 0.0", float(log(world->getContactSolver().getConfig().error_to_terminate)), 1.0f, ImVec2(500,300));
const auto* problem = world->getContactProblem();
for(int i=0; i<problem->size(); i++) {
ImGui::Text("%i: Rank %i", i, problem->at(i).rank);
}
ImGui::PopFont();
}
if (ImGui::CollapsingHeader("Video recording")) {
if(vis->isRecording()) {
ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0.2f, 0.8f, 0.2f, 1.f));
ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImVec4(0.3f, 0.9f, 0.3f, 1.f));
ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImVec4(0.5f, 0.9f, 0.5f, 1.f));
if(ImGui::Button("Stop Recording ")) {
RSINFO("Stop recording")
raisim::OgreVis::get()->stopRecordingVideoAndSave();
}
ImGui::PopStyleColor(3);
} else {
ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0.8f, 0.2f, 0.2f, 1.f));
ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImVec4(0.9f, 0.3f, 0.3f, 1.f));
ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImVec4(0.9f, 0.5f, 0.5f, 1.f));
if(ImGui::Button("Record ")){
RSINFO("Start recording")
raisim::OgreVis::get()->startRecordingVideo(raisim::OgreVis::get()->getResourceDir() + "/test.mp4");
}
ImGui::PopStyleColor(3);
}
}
if (ImGui::CollapsingHeader("Key maps")) {
ImGui::Text("F1~4 : toggle visualization mask\n");
ImGui::Text("Mouse L : orbital mode\n");
ImGui::Text("Mouse R : free cam mode\n");
ImGui::Text("Shift : pan during free cam mode\n");
}
const float INDENT = ImGui::GetTreeNodeToLabelSpacing();
if (ImGui::CollapsingHeader("Object List")) {
ImGui::Indent(INDENT);
auto& items = vis->getObjectSet();
for(auto& it: items.set) {
if(ImGui::TreeNode(it.first->getName().c_str())) {
ImGui::PushFont(fontMid);
ImGui::Indent(INDENT);
if(ImGui::TreeNode("simulation objects")) {
ImGui::Indent(INDENT);
ImGui::Unindent(INDENT);
ImGui::TreePop();
}
if(ImGui::TreeNode("visual objects")) {
ImGui::Indent(INDENT);
ImGui::Unindent(INDENT);
ImGui::TreePop();
}
ImGui::Unindent(INDENT);
ImGui::PopFont();
ImGui::TreePop();
}
}
ImGui::Unindent(INDENT);
}
ImGui::End();
}
void imguiSetupCallback() {
#define HI(v) ImVec4(0.502f, 0.075f, 0.256f, v)
#define MED(v) ImVec4(0.455f, 0.198f, 0.301f, v)
#define LOW(v) ImVec4(0.232f, 0.201f, 0.271f, v)
// backgrounds (@todo: complete with BG_MED, BG_LOW)
#define BG(v) ImVec4(0.200f, 0.220f, 0.270f, v)
// text
#define TEXT(v) ImVec4(0.860f, 0.930f, 0.890f, v)
auto &style = ImGui::GetStyle();
style.Alpha = 0.8;
style.Colors[ImGuiCol_Text] = TEXT(0.78f);
style.Colors[ImGuiCol_TextDisabled] = TEXT(0.28f);
style.Colors[ImGuiCol_WindowBg] = ImVec4(0.13f, 0.14f, 0.17f, 1.00f);
style.Colors[ImGuiCol_ChildWindowBg] = BG( 0.58f);
style.Colors[ImGuiCol_PopupBg] = BG( 0.9f);
style.Colors[ImGuiCol_Border] = ImVec4(0.31f, 0.31f, 1.00f, 0.00f);
style.Colors[ImGuiCol_BorderShadow] = ImVec4(0.00f, 0.00f, 0.00f, 0.00f);
style.Colors[ImGuiCol_FrameBg] = BG( 1.00f);
style.Colors[ImGuiCol_FrameBgHovered] = MED( 0.78f);
style.Colors[ImGuiCol_FrameBgActive] = MED( 1.00f);
style.Colors[ImGuiCol_TitleBg] = LOW( 1.00f);
style.Colors[ImGuiCol_TitleBgActive] = HI( 1.00f);
style.Colors[ImGuiCol_TitleBgCollapsed] = BG( 0.75f);
style.Colors[ImGuiCol_MenuBarBg] = BG( 0.47f);
style.Colors[ImGuiCol_ScrollbarBg] = BG( 1.00f);
style.Colors[ImGuiCol_ScrollbarGrab] = ImVec4(0.09f, 0.15f, 0.16f, 1.00f);
style.Colors[ImGuiCol_ScrollbarGrabHovered] = MED( 0.78f);
style.Colors[ImGuiCol_ScrollbarGrabActive] = MED( 1.00f);
style.Colors[ImGuiCol_CheckMark] = ImVec4(0.71f, 0.22f, 0.27f, 1.00f);
style.Colors[ImGuiCol_SliderGrab] = ImVec4(0.47f, 0.77f, 0.83f, 0.14f);
style.Colors[ImGuiCol_SliderGrabActive] = ImVec4(0.71f, 0.22f, 0.27f, 1.00f);
style.Colors[ImGuiCol_Button] = ImVec4(0.47f, 0.77f, 0.83f, 0.14f);
style.Colors[ImGuiCol_ButtonHovered] = MED( 0.86f);
style.Colors[ImGuiCol_ButtonActive] = MED( 1.00f);
style.Colors[ImGuiCol_Header] = MED( 0.76f);
style.Colors[ImGuiCol_HeaderHovered] = MED( 0.86f);
style.Colors[ImGuiCol_HeaderActive] = HI( 1.00f);
style.Colors[ImGuiCol_Column] = ImVec4(0.14f, 0.16f, 0.19f, 1.00f);
style.Colors[ImGuiCol_ColumnHovered] = MED( 0.78f);
style.Colors[ImGuiCol_ColumnActive] = MED( 1.00f);
style.Colors[ImGuiCol_ResizeGrip] = ImVec4(0.47f, 0.77f, 0.83f, 0.04f);
style.Colors[ImGuiCol_ResizeGripHovered] = MED( 0.78f);
style.Colors[ImGuiCol_ResizeGripActive] = MED( 1.00f);
style.Colors[ImGuiCol_PlotLines] = TEXT(0.63f);
style.Colors[ImGuiCol_PlotLinesHovered] = MED( 1.00f);
style.Colors[ImGuiCol_PlotHistogram] = TEXT(0.63f);
style.Colors[ImGuiCol_PlotHistogramHovered] = MED( 1.00f);
style.Colors[ImGuiCol_TextSelectedBg] = MED( 0.43f);
// [...]
style.Colors[ImGuiCol_ModalWindowDarkening] = BG( 0.73f);
style.WindowPadding = ImVec2(6, 4);
style.WindowRounding = 0.0f;
style.FramePadding = ImVec2(5, 2);
style.FrameRounding = 3.0f;
style.ItemSpacing = ImVec2(7, 1);
style.ItemInnerSpacing = ImVec2(1, 1);
style.TouchExtraPadding = ImVec2(0, 0);
style.IndentSpacing = 6.0f;
style.ScrollbarSize = 12.0f;
style.ScrollbarRounding = 16.0f;
style.GrabMinSize = 20.0f;
style.GrabRounding = 2.0f;
style.WindowTitleAlign.x = 0.50f;
style.Colors[ImGuiCol_Border] = ImVec4(0.539f, 0.479f, 0.255f, 0.162f);
style.FrameBorderSize = 0.0f;
style.WindowBorderSize = 1.0f;
ImGuiIO &io = ImGui::GetIO();
fontBig = io.Fonts->AddFontFromFileTTF((raisim::OgreVis::get()->getResourceDir() + "/font/DroidSans.ttf").c_str(), 25.0f);
fontMid = io.Fonts->AddFontFromFileTTF((raisim::OgreVis::get()->getResourceDir() + "/font/DroidSans.ttf").c_str(), 22.0f);
fontSmall = io.Fonts->AddFontFromFileTTF((raisim::OgreVis::get()->getResourceDir() + "/font/DroidSans.ttf").c_str(), 16.0f);
}
#endif //RAISIMOGREVISUALIZER_RAISIMBASICIMGUIPANEL_HPP
pyrobolearn/simulators/raisim_wrapper/include/visualizer/raisimKeyboardCallback.hpp
-54
View File
@@ -1,54 +0,0 @@
//
// Created by Jemin Hwangbo on 4/11/19.
// MIT License
//
// Copyright (c) 2019-2019 Robotic Systems Lab, ETH Zurich
//
// 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 RAISIMOGREVISUALIZER_RAISIMKEYBOARDCALLBACK_HPP
#define RAISIMOGREVISUALIZER_RAISIMKEYBOARDCALLBACK_HPP
#include "raisim/OgreVis.hpp"
#include "raisimKeyboardCallback.hpp"
#include "guiState.hpp"
bool raisimKeyboardCallback(const OgreBites::KeyboardEvent &evt) {
auto &key = evt.keysym.sym;
// termination gets the highest priority
switch (key) {
case OgreBites::SDLK_F1:
raisim::gui::showBodies = !raisim::gui::showBodies;
break;
case OgreBites::SDLK_F2:
raisim::gui::showCollision = !raisim::gui::showCollision;
break;
case OgreBites::SDLK_F3:
raisim::gui::showContacts = !raisim::gui::showContacts;
break;
case OgreBites::SDLK_F4:
raisim::gui::showForces = !raisim::gui::showForces;
break;
default:
break;
}
return false;
}
#endif //RAISIMOGREVISUALIZER_RAISIMKEYBOARDCALLBACK_HPP
pyrobolearn/simulators/raisim_wrapper/include/visualizer/visSetupCallback.hpp
-59
View File
@@ -1,59 +0,0 @@
//
// Created by jemin on 5/16/19.
// MIT License
//
// Copyright (c) 2019-2019 Robotic Systems Lab, ETH Zurich
//
// 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 RAISIMOGREVISUALIZER_SETUPCALLBACK_HPP
#define RAISIMOGREVISUALIZER_SETUPCALLBACK_HPP
#include <raisim/OgreVis.hpp>
void setupCallback() {
auto vis = raisim::OgreVis::get();
/// light
vis->getLight()->setDiffuseColour(1, 1, 1);
vis->getLight()->setCastShadows(true);
Ogre::Vector3 lightdir(-3,-3,-0.5);
lightdir.normalise();
vis->getLightNode()->setDirection({lightdir});
vis->setCameraSpeed(300);
/// load textures
vis->addResourceDirectory(vis->getResourceDir() + "/material/checkerboard");
vis->loadMaterialFile("checkerboard.material");
/// shdow setting
vis->getSceneManager()->setShadowTechnique(Ogre::SHADOWTYPE_TEXTURE_ADDITIVE);
vis->getSceneManager()->setShadowTextureSettings(2048, 3);
/// scale related settings!! Please adapt it depending on your map size
// beyond this distance, shadow disappears
vis->getSceneManager()->setShadowFarDistance(3);
// size of contact points and contact forces
vis->setContactVisObjectSize(0.03, 0.2);
// speed of camera motion in freelook mode
vis->getCameraMan()->setTopSpeed(5);
}
#endif //RAISIMOGREVISUALIZER_SETUPCALLBACK_HPP
pyrobolearn/simulators/raisim_wrapper/src/articulated_system.cpp
-157
View File
@@ -1,157 +0,0 @@
/**
* Python wrappers for raisim.object.ArticulatedSystem using pybind11.
*
* Copyright (c) 2019, jhwangbo (C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc.
#include "raisim/object/ArticulatedSystem/loaders.hpp"
#include "raisim/object/ArticulatedSystem/JointAndBodies.hpp"
#include "raisim/object/ArticulatedSystem/ArticulatedSystem.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_articulated_system(py::module &m) {
/****************/
/* LoadFromMJCF */
/****************/
py::class_<raisim::mjcf::LoadFromMJCF>(m, "LoadFromMJCF", "Load from MJCF file.")
.def(py::init<ArticulatedSystem &, std::string, std::vector<std::string>>(), "Initialize the MJCF loader.");
/*****************/
/* LoadFromURDF2 */
/*****************/
py::class_<raisim::urdf::LoadFromURDF2>(m, "LoadFromURDF2", "Load from URDF file.")
.def(py::init<ArticulatedSystem &, std::string, std::vector<std::string>>(), "Initialize the URDF loader.");
/***************/
/* ControlMode */
/***************/
py::enum_<raisim::ControlMode::Type>(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_<raisim::ArticulatedSystemOption>(m, "ArticulatedSystemOption", "Articulated System Option.")
.def_readwrite("do_not_collide_with_parent", &raisim::ArticulatedSystemOption::doNotCollideWithParent);
/*********************/
/* ArticulatedSystem */
/*********************/
// From the `ArticulatedSystem.h` file:
/* list of vocabs
1. body: body here refers to only rotating bodies. Fixed bodies are optimized out.
Position of a body refers to the position of the joint connecting the body and its parent.
2. Coordinate frame: coordinate frames are defined on every joint (even at the fixed joints). If you want
to define a custom frame, define a fixed zero-mass object and a joint in the URDF */
py::class_<raisim::ArticulatedSystem, raisim::Object> system(m, "ArticulatedSystem", "Raisim Articulated System.");
py::enum_<raisim::ArticulatedSystem::Frame>(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);
system.def(py::init<>(), "Initialize the Articulated System.")
.def(py::init<const std::string &, const std::string &, std::vector<std::string>, raisim::ArticulatedSystemOption>(),
"Initialize the Articulated System.\n\n"
"Do not call this method yourself. use World class to create an Articulated system.\n\n"
"Args:\n"
" filename (str): path to the robot description file (URDF, etc).\n"
" resource_directory (str): path the resource directory. If empty, it will use the robot description folder.\n"
" joint_order (list[str]): specify the joint order, if we want it to be different from the URDF file.\n"
" options (ArticulatedSystemOption): options.",
py::arg("filename"), py::arg("resource_directory"), py::arg("joint_order"), py::arg("options"))
.def("get_generalized_coordinate", [](raisim::ArticulatedSystem &self) {
return convert_vecdyn_to_np(self.getGeneralizedCoordinate());
}, R"mydelimiter(
Get the generalized coordinates of the system.
Returns:
np.array[float[n]]: generalized coordinates.
)mydelimiter")
.def("get_base_orientation", [](raisim::ArticulatedSystem &self) {
Vec<4> quaternion;
self.getBaseOrientation(quaternion);
return convert_vec_to_np(quaternion);
}, R"mydelimiter(
Get the base orientation (expressed as a quaternion [w,x,y,z]).
Returns:
np.array[float[4]]: base orientation (expressed as a quaternion [w,x,y,z])
)mydelimiter")
.def("get_base_rotation_matrix", [](raisim::ArticulatedSystem &self) {
Mat<3,3> rot;
self.getBaseOrientation(rot);
return convert_mat_to_np(rot);
}, R"mydelimiter(
Get the base orientation (expressed as a rotation matrix).
Returns:
np.array[float[3,3]]: rotation matrix
)mydelimiter")
.def("get_generalized_velocity", [](raisim::ArticulatedSystem &self) {
return convert_vecdyn_to_np(self.getGeneralizedVelocity());
}, R"mydelimiter(
Get the generalized velocities of the system.
Returns:
np.array[float[n]]: generalized velocities.
)mydelimiter")
.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")
;
}
pyrobolearn/simulators/raisim_wrapper/src/constraints.cpp
-230
View File
@@ -1,230 +0,0 @@
/**
* Python wrappers for raisim.constraints using pybind11.
*
* Copyright (c) 2019, jhwangbo (C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // 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_<raisim::Constraints>(constraints_module, "Constraints", "Raisim Constraints from which all other constraints inherit from.");
/********/
/* Wire */
/********/
py::class_<raisim::Wire, raisim::Constraints>(constraints_module, "Wire", "Raisim Wire constraint class; it creates a wire constraint between 2 bodies.")
.def(py::init([](raisim::Object &object1, size_t local_idx1, py::array_t<double> pos_body1,
raisim::Object &object2, size_t local_idx2, py::array_t<double> 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
return new 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.",
py::arg("object1"), py::arg("local_idx1"), py::arg("pos_body1"), py::arg("object2"), py::arg("local_idx2"),
py::arg("pos_body2"), py::arg("length"))
.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::Wire::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_<raisim::StiffWire, raisim::Wire>(constraints_module, "StiffWire", "Raisim StiffWire constraint class; it creates a stiff wire constraint between 2 bodies.")
.def(py::init([](raisim::Object &object1, size_t local_idx1, py::array_t<double> pos_body1,
raisim::Object &object2, size_t local_idx2, py::array_t<double> 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
return new 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.",
py::arg("object1"), py::arg("local_idx1"), py::arg("pos_body1"), py::arg("object2"), py::arg("local_idx2"),
py::arg("pos_body2"), py::arg("length"));
/*****************/
/* CompliantWire */
/*****************/
py::class_<raisim::CompliantWire, raisim::Wire>(constraints_module, "CompliantWire", "Raisim Compliant Wire constraint class; it creates a compliant wire constraint between 2 bodies.")
.def(py::init([](raisim::Object &object1, size_t local_idx1, py::array_t<double> pos_body1,
raisim::Object &object2, size_t local_idx2, py::array_t<double> 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
return new 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.",
py::arg("object1"), py::arg("local_idx1"), py::arg("pos_body1"), py::arg("object2"), py::arg("local_idx2"),
py::arg("pos_body2"), py::arg("length"), py::arg("stiffness"))
.def("apply_tension", &raisim::CompliantWire::applyTension, "Apply a tension in the compliant wire.");
}
pyrobolearn/simulators/raisim_wrapper/src/contact.cpp
-429
View File
@@ -1,429 +0,0 @@
/**
* Python wrappers for raisim.contact using pybind11.
*
* Copyright (c) 2019, kangd and jhwangbo (C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // 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_<raisim::contact::Contact>(contact_module, "Contact", "Raisim Contact.")
.def(py::init([](py::array_t<double> position, py::array_t<double> 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
return new 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<double> 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<double> 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.");
/**************************/
/* Single3DContactProblem */
/**************************/
py::class_<raisim::contact::Single3DContactProblem>(contact_module, "Single3DContactProblem", "Raisim single 3D contact problem.")
.def(py::init<>(), "Initialize the single 3D contact problem.")
.def(py::init<const MaterialPairProperties&, double, double, double, double>(),
"Initialize the single 3D contact problem.\n\n"
"Args:\n"
" material_properties (MaterialPairProperties): material pair properties.\n"
" x (float): x position.\n"
" y (float): y position.\n"
" z (float): z position.\n"
" depth_in (float): penetration depth.",
py::arg("material_properties"), py::arg("x"), py::arg("y"), py::arg("z"), py::arg("depth_in"))
.def("check_rank", &raisim::contact::Single3DContactProblem::checkRank)
.def_property("imp_i",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_vec_to_np(self.imp_i);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Vec<3> vec = convert_np_to_vec<3>(array);
self.imp_i = vec;
})
.def_property("tau_i",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_vec_to_np(self.tau_i);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Vec<3> vec = convert_np_to_vec<3>(array);
self.tau_i = vec;
})
.def_property("position_W",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_vec_to_np(self.position_W);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Vec<3> vec = convert_np_to_vec<3>(array);
self.position_W = vec;
})
.def_property("MappInv_i",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_mat_to_np(self.MappInv_i);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Mat<3, 3> mat = convert_np_to_mat<3, 3>(array);
self.MappInv_i = mat;
})
.def_property("MappInvWODel_i",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_mat_to_np(self.MappInvWODel_i);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Mat<3, 3> mat = convert_np_to_mat<3, 3>(array);
self.MappInvWODel_i = mat;
})
.def_property("Mapp_i",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_mat_to_np(self.Mapp_i);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Mat<3, 3> mat = convert_np_to_mat<3, 3>(array);
self.Mapp_i = mat;
})
.def_property("Mapp_iInv22",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_mat_to_np(self.Mapp_iInv22);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Mat<2, 2> mat = convert_np_to_mat<2, 2>(array);
self.Mapp_iInv22 = mat;
})
.def_property("Mapp_i22",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_mat_to_np(self.Mapp_i22);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Mat<2, 2> mat = convert_np_to_mat<2, 2>(array);
self.Mapp_i22 = mat;
})
.def_property("cooTrans_i",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_vec_to_np(self.cooTrans_i);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Vec<3> vec = convert_np_to_vec<3>(array);
self.cooTrans_i = vec;
})
.def_property("MappInv_red",
[](raisim::contact::Single3DContactProblem &self) { // getter
return convert_mat_to_np(self.MappInv_red);
}, [](raisim::contact::Single3DContactProblem &self, py::array_t<double> array) { // setter
Mat<3, 2> mat = convert_np_to_mat<3, 2>(array);
self.MappInv_red = mat;
})
.def_property("MappInv_j",
[](raisim::contact::Single3DContactProblem &self) { // getter (avoid to use this one as we have to copy everything)
py::list list;
for (auto elem : self.MappInv_j)
list.append(convert_mat_to_np(elem));
return list;
}, [](raisim::contact::Single3DContactProblem &self, py::list list) { // setter (avoid to use this one as possible as we have to copy everything)
std::vector<raisim::Mat<3,3>> vector;
for (auto elem : list) {
py::array_t<double> e = elem.cast<py::array_t<double>>();
vector.push_back(convert_np_to_mat<3, 3>(e));
}
self.MappInv_j = vector;
})
.def_property("imp_j",
[](raisim::contact::Single3DContactProblem &self) { // getter (avoid to use this one as we have to copy everything)
py::list list;
for (auto elem : self.imp_j)
list.append(convert_vec_to_np(*elem));
return list;
}, [](raisim::contact::Single3DContactProblem &self, py::list list) { // setter (avoid to use this one as possible as we have to copy everything)
std::vector<raisim::Vec<3> *> vector;
for (auto elem : list) {
py::array_t<double> e = elem.cast<py::array_t<double>>();
auto item = convert_np_to_vec<3>(e);
vector.push_back(&item);
}
self.imp_j = vector;
})
.def_readwrite("mu", &raisim::contact::Single3DContactProblem::mu)
.def_readwrite("n2_mu", &raisim::contact::Single3DContactProblem::n2_mu)
.def_readwrite("muinv", &raisim::contact::Single3DContactProblem::muinv)
.def_readwrite("negMuSquared", &raisim::contact::Single3DContactProblem::negMuSquared)
.def_readwrite("coeffRes", &raisim::contact::Single3DContactProblem::coeffRes)
.def_readwrite("bounceThres", &raisim::contact::Single3DContactProblem::bounceThres)
.def_readwrite("Mapp_iInv11", &raisim::contact::Single3DContactProblem::Mapp_iInv11)
.def_readwrite("impact_vel", &raisim::contact::Single3DContactProblem::impact_vel)
.def_readwrite("depth", &raisim::contact::Single3DContactProblem::depth)
.def_readwrite("rank", &raisim::contact::Single3DContactProblem::rank)
.def_readwrite("obA", &raisim::contact::Single3DContactProblem::obA)
.def_readwrite("obB", &raisim::contact::Single3DContactProblem::obB)
.def_readwrite("point_idA", &raisim::contact::Single3DContactProblem::pointIdA)
.def_readwrite("point_idB", &raisim::contact::Single3DContactProblem::pointIdB)
.def_readwrite("atLeastOneWithoutDel", &raisim::contact::Single3DContactProblem::atLeastOneWithoutDel);
/**************************/
/* BisectionContactSolver */
/**************************/
py::class_<raisim::contact::BisectionContactSolver> solver(contact_module, "BisectionContactSolver", "Raisim Bisection Contact Solver.");
py::class_<raisim::contact::BisectionContactSolver::SolverConfiguration>(solver, "SolverConfiguration", "Raisim solver configuration struct.")
.def_readwrite("alpha_init", &raisim::contact::BisectionContactSolver::SolverConfiguration::alpha_init)
.def_readwrite("alpha_low", &raisim::contact::BisectionContactSolver::SolverConfiguration::alpha_low)
.def_readwrite("alpha_decay", &raisim::contact::BisectionContactSolver::SolverConfiguration::alpha_decay)
.def_readwrite("error_to_terminate", &raisim::contact::BisectionContactSolver::SolverConfiguration::error_to_terminate)
.def_readwrite("erp", &raisim::contact::BisectionContactSolver::SolverConfiguration::erp)
.def_readwrite("erp2", &raisim::contact::BisectionContactSolver::SolverConfiguration::erp2)
.def_readwrite("maxIteration", &raisim::contact::BisectionContactSolver::SolverConfiguration::maxIteration);
solver.def(py::init<double>(), "Initialize the bisection contact solver.", py::arg("dt"))
.def("solve", &raisim::contact::BisectionContactSolver::solve, py::arg("contact"))
.def("update_config", &raisim::contact::BisectionContactSolver::updateConfig, py::arg("config"))
.def("set_time_step", &raisim::contact::BisectionContactSolver::setTimestep, py::arg("dt"))
.def("set_order", &raisim::contact::BisectionContactSolver::setOrder, py::arg("order"))
.def("get_loop_counter", &raisim::contact::BisectionContactSolver::getLoopCounter)
.def("get_error_history", &raisim::contact::BisectionContactSolver::getErrorHistory)
.def("get_config", py::overload_cast<>(&raisim::contact::BisectionContactSolver::getConfig))
.def("get_config", py::overload_cast<>(&raisim::contact::BisectionContactSolver::getConfig, py::const_));
/************************/
/* PerObjectContactList */
/************************/
py::class_<raisim::contact::PerObjectContactList>(contact_module, "PerObjectContactList",
"Raisim PerObjectContactList, where each object has a list of Contacts.")
.def(py::init<>(), "Initialize the per object contact list.")
.def("add_contact", &raisim::contact::PerObjectContactList::addContact)
.def("clear_contacts", &raisim::contact::PerObjectContactList::clearContacts)
.def("get_num_contacts", &raisim::contact::PerObjectContactList::getNumContacts)
.def("get_contact_at", &raisim::contact::PerObjectContactList::getContactAt, py::arg("index"))
.def("get_contacts", py::overload_cast<>(&raisim::contact::PerObjectContactList::getContacts))
.def("get_contacts", py::overload_cast<>(&raisim::contact::PerObjectContactList::getContacts, py::const_))
.def("save_impulses_for_warm_start", &raisim::contact::PerObjectContactList::saveImpulsesForWarmStart)
.def("warm_start", &raisim::contact::PerObjectContactList::warmStart)
.def("get_impact_velocities", &raisim::contact::PerObjectContactList::getImpactVel);
}
pyrobolearn/simulators/raisim_wrapper/src/converter.cpp
-213
View File
@@ -1,213 +0,0 @@
/**
* Type converters used to convert between different data types.
*
* Copyright (c) 2019, Brian Delhaisse <briandelhaisse@gmail.com>
*
* 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<double> 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<double>(
{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<double> 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<size; i++) {
vec[i] = *array.data(i);
}
// return vector
return vec;
}
/// \brief: convert from raisim::MatDyn to np.array[float64[n,m]]
py::array_t<double> 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<double>(
{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<double> 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<nrows; i++)
for (size_t j=0; j<ncols; j++)
mat[i, j] = *array.data(i, j);
// return matrix
return mat;
}
/// \brief: convert from raisim::Transformation to np.array[float[4,4]]
py::array_t<double> convert_transformation_to_np(const raisim::Transformation &transfo) {
// convert from vec / mat to np.array
// auto pos = convert_vec_to_np(transfo.pos);
// auto rot = convert_mat_to_np(transfo.rot);
auto pos = transfo.pos;
auto rot = transfo.rot;
// create 4x4 matrix
py::array_t<double> homogeneous({4, 4});
// fill for rotation matrix
for (size_t i=0; i<3; i++)
for (size_t j=0; j<3; j++)
*homogeneous.mutable_data(i, j) = rot[i,j]; // *rot.data(i, j);
// fill for position vector
for (size_t i=0; i<3; i++)
*homogeneous.mutable_data(i, 3) = pos[i]; //*pos.data(i);
// fill zero for last row
for (size_t j=0; j<3; j++)
*homogeneous.mutable_data(3, j) = 0;
// fill 1 for the last cell (last row and last column)
*homogeneous.mutable_data(4,4) = 1;
return homogeneous;
}
/// \brief: convert from np.array[float[4,4]] to raisim::Transformation
raisim::Transformation convert_np_to_transformation(py::array_t<double> 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) != 4 || array.shape(1) != 4) {
std::ostringstream s;
s << "error: expecting the given array to have a shape (4,4), but got instead a shape of ("
<< array.shape(0) << "," << array.shape(1) << ").";
throw std::domain_error(s.str());
}
// create transformation
raisim::Transformation transfo;
raisim::Mat<3,3> rot = transfo.rot;
raisim::Vec<3> pos = transfo.pos;
// fill rotation matrix
for (size_t i=0; i<3; i++)
for (size_t j=0; j<3; j++)
rot[i, j] = *array.data(i, j);
// fill position vector
for (size_t i=0; i<3; i++)
pos[i] = *array.data(i, 3);
return transfo;
}
/// \brief: convert from Eigen::Quaterniond to np.array[float[4]]
py::array_t<double> convert_quaternion_to_np(const Eigen::Quaterniond &quaternion) {
// create vector of size 4
py::array_t<double> array({4});
// fill quaternion
*array.mutable_data(0) = quaternion.w();
*array.mutable_data(1) = quaternion.x();
*array.mutable_data(2) = quaternion.y();
*array.mutable_data(3) = quaternion.z();
// return quaternion
return array;
}
/// \brief: convert from np.array[float[4]] to Eigen::Quaterniond
Eigen::Quaterniond convert_np_to_quaternion(py::array_t<double> array) {
// check dimensions and shape
if (array.size() != 4) {
std::ostringstream s;
s << "error: expecting the given array to have a size of 4, but got instead a size of "
<< array.size() << ".";
throw std::domain_error(s.str());
}
// reshape if necessary
if (array.ndim() > 1)
array.resize({array.size()});
// create quaternion
Eigen::Quaterniond quaternion(*array.data(0), *array.data(1), *array.data(2), *array.data(3));
// return quaternion
return quaternion;
}
pyrobolearn/simulators/raisim_wrapper/src/materials.cpp
-97
View File
@@ -1,97 +0,0 @@
/**
* Python wrappers for raisim.Materials using pybind11.
*
* Copyright (c) 2019, jhwangbo (C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
#include <iostream>
#include "raisim/Materials.hpp"
namespace py = pybind11;
using namespace raisim;
void init_materials(py::module &m) {
/**************************/
/* MaterialPairProperties */
/**************************/
py::class_<raisim::MaterialPairProperties>(m, "MaterialPairProperties", "Raisim Material Pair Properties (friction and restitution).")
.def(py::init<>(), "Initialize the material pair properties.")
.def(py::init<double, double, double>(),
"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"));
/*******************/
/* MaterialManager */
/*******************/
py::class_<raisim::MaterialManager>(m, "MaterialManager", "Raisim Material Manager.")
.def(py::init<>(), "Initialize the material pair manager.")
.def(py::init<const std::string>(),
"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"));
}
pyrobolearn/simulators/raisim_wrapper/src/math.cpp
-70
View File
@@ -1,70 +0,0 @@
/**
* Python wrappers for raisim.math using pybind11. Have alsso a look at
* `converter.hpp` and `converter.cpp` which contains the code to convert between
* np.array to raisim::Vec, raisim::Mat, raisim::VecDyn, raisim::MatDyn, and
* raisim::Transformation.
*
* Copyright (c) 2019, jhwangbo (C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
#include "raisim/math.hpp" // contains the definitions of Vec, Mat, Transformation, etc.
#include "converter.hpp" // contains code that allows to convert between the Vec, Mat to numpy arrays.
namespace py = pybind11;
using namespace raisim;
void init_math(py::module &m) {
/********/
/* Math */
/********/
// the code to convert between the data types is in `converter.hpp` and `converter.cpp`.
py::class_<raisim::Transformation>(m, "Transformation", "Raisim homogeneous transformation.")
.def(py::init<>()) // default constructor
.def_property("rot",
[](raisim::Transformation &self) { // getter
// convert from Mat<3,3> to np.array[3,3]
return convert_mat_to_np(self.rot);
}, [](raisim::Transformation &self, py::array_t<double> array) { // setter
// convert from np.array[3,3] to Mat<3,3>
Mat<3,3> rot = convert_np_to_mat<3,3>(array);
self.rot = rot;
})
.def_property("pos",
[](raisim::Transformation &self) { // getter
// convert from Vec<3> to np.array[3]
return convert_vec_to_np(self.pos);
}, [](raisim::Transformation &self, py::array_t<double> array) { // setter
// convert from np.array[3] to Vec<3>
Vec<3> pos = convert_np_to_vec<3>(array);
self.pos = pos;
});
}
pyrobolearn/simulators/raisim_wrapper/src/object.cpp
-208
View File
@@ -1,208 +0,0 @@
/**
* Python wrappers for raisim.object using pybind11.
*
* Copyright (c) 2019, kangd (original C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
#include "raisim/object/Object.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 &);
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 object submodule.");
/**************/
/* ObjectType */
/**************/
// object type enum (from include/raisim/configure.hpp)
py::enum_<raisim::ObjectType>(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_<raisim::BodyType>(object_module, "BodyType", py::arithmetic())
.value("STATIC", raisim::BodyType::STATIC)
.value("KINEMATIC", raisim::BodyType::KINEMATIC)
.value("DYNAMIC", raisim::BodyType::DYNAMIC);
/**********/
/* Object */
/**********/
py::class_<raisim::Object>(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, py::arg("contact"))
.def("set_index_in_world", &raisim::Object::setIndexInWorld, py::arg("index"))
.def("get_index_in_world", &raisim::Object::getIndexInWorld)
.def("get_contacts", py::overload_cast<>(&raisim::Object::getContacts))
.def("get_contacts", py::overload_cast<>(&raisim::Object::getContacts, py::const_))
.def("update_collision", &raisim::Object::updateCollision, "Update the collisions.")
.def("prec_contact_solver_update1", [](raisim::Object &self, py::array_t<double> gravity, double dt) {
// convert np.array[3] to Vec<3>
Vec<3> vec = convert_np_to_vec<3>(gravity);
self.preContactSolverUpdate1(vec, dt);
}, py::arg("gravity"), py::arg("dt"))
.def("prec_contact_solver_update2", [](raisim::Object &self, py::array_t<double> gravity, double dt) {
// convert np.array[3] to Vec<3>
Vec<3> vec = convert_np_to_vec<3>(gravity);
self.preContactSolverUpdate1(vec, dt);
}, py::arg("gravity"), py::arg("dt"))
.def("integrate", &raisim::Object::integrate, "integrate.", py::arg("dt"))
.def("set_external_force", [](raisim::Object &self, size_t local_idx, py::array_t<double> force) {
// convert np.array[3] to Vec<3>
Vec<3> f = convert_np_to_vec<3>(force);
self.setExternalForce(local_idx, f);
}, R"mydelimiter(
Set the external force on the body.
Args:
local_idx (int): local index.
force (np.array[float[3]]): force vector.
)mydelimiter",
py::arg("local_idx"), py::arg("force"))
.def("set_external_torque", [](raisim::Object &self, size_t local_idx, py::array_t<double> torque) {
// convert np.array[3] to Vec<3>
Vec<3> t = convert_np_to_vec<3>(torque);
self.setExternalTorque(local_idx, t);
}, R"mydelimiter(
Set the external force on the body.
Args:
local_idx (int): local index.
force (np.array[float[3]]): force vector.
)mydelimiter",
py::arg("local_idx"), py::arg("torque"))
.def("get_mass", &raisim::Object::getMass, py::arg("local_idx"))
.def("get_object_type", &raisim::Object::getObjectType)
.def("get_world_position", [](raisim::Object &self, size_t local_idx) {
Vec<3> pos;
self.getPosition_W(local_idx, pos);
// convert vec<3> to np.array[3]
return convert_vec_to_np(pos);
}, R"mydelimiter(
Get the world position.
Args:
local_idx (int): local index.
Returns:
np.array[float[3]]: position expressed in the Cartesian world frame.
)mydelimiter",
py::arg("local_idx"))
.def("get_world_linear_velocity", [](raisim::Object &self, size_t local_idx) {
Vec<3> vel;
self.getVelocity_W(local_idx, vel);
return convert_vec_to_np(vel);
}, R"mydelimiter(
Get the world linear velocity.
Args:
local_idx (int): local index.
Returns:
np.array[float[3]]: linear velocity expressed in the Cartesian world frame.
)mydelimiter",
py::arg("local_idx"))
.def("get_world_rotation_matrix", [](raisim::Object &self, size_t local_idx) {
Mat<3,3> rot;
self.getOrientation_W(local_idx, rot);
return convert_mat_to_np(rot);
}, R"mydelimiter(
Get the world orientation as a rotation matrix.
Args:
local_idx (int): local index.
Returns:
np.array[float[3,3]]: rotation matrix.
)mydelimiter",
py::arg("local_idx"))
.def("get_world_position", [](raisim::Object &self, size_t local_idx, py::array_t<double> body_pos) {
Vec<3> pos_b =convert_np_to_vec<3>(body_pos);
Vec<3> pos;
self.getPosition_W(local_idx, pos_b, pos);
return convert_vec_to_np(pos);
})
.def("get_body_type", py::overload_cast<size_t>(&raisim::Object::getBodyType, py::const_))
.def("get_body_type", py::overload_cast<>(&raisim::Object::getBodyType, py::const_))
.def("get_contact_point_velocity", [](raisim::Object &self, size_t point_id) {
Vec<3> vel;
self.getContactPointVel(point_id, vel);
return convert_vec_to_np(vel);
})
;
// raisim.object.singleBodies
init_single_bodies(object_module);
// raisim.object.ArticulatedSystem
init_articulated_system(object_module);
// raisim.object.terrain
init_terrain(object_module);
}
pyrobolearn/simulators/raisim_wrapper/src/raisim_wrapper.cpp
-96
View File
@@ -1,96 +0,0 @@
/**
* Python wrappers for RaiSim using pybind11.
*
* Copyright (c) 2019, Brian Delhaisse <briandelhaisse@gmail.com>
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
#include <iostream>
#include "ode/ode.h"
#include "ode/collision.h"
namespace py = pybind11;
void init_math(py::module &);
void init_materials(py::module &);
void init_object(py::module &);
void init_constraints(py::module &);
void init_contact(py::module &);
void init_world(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
/*******/
/* Ode */
/*******/
/* collision space from ode (in ode/common.h), used to define dSpaceID */
py::class_<dSpaceID>(m, "dSpaceID", "collision space from ode (in ode/common.h).");
/* geometry collision object from ode (in ode/common.h), used to define dGeomID */
py::class_<dGeomID>(m, "dGeomID", "geometry collision object from ode (in ode/common.h).");
/********/
/* Math */
/********/
init_math(m);
/*************/
/* Materials */
/*************/
init_materials(m);
/******************/
/* raisim.contact */
/******************/
init_contact(m);
/*****************/
/* raisim.object */
/*****************/
init_object(m); // define primitive shapes and articulated systems)
/*********************/
/* raisim.constraint */
/*********************/
init_constraints(m);
/*********/
/* World */
/*********/
init_world(m);
/*********************/
/* raisim.visualizer */
/*********************/
init_visualizer(m);
}
pyrobolearn/simulators/raisim_wrapper/src/single_bodies.cpp
-647
View File
@@ -1,647 +0,0 @@
/**
* Python wrappers for raisim.object using pybind11.
*
* Copyright (c) 2019, kangd and jhwangbo (original C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
//#include <pybind11/numpy.h> // 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 "ode/collision.h"
#include "ode/ode.h"
#include "ode/extras/collision_kernel.h"
// Important note: for the above include ("ode/src/collision_kernel.h"), you have to add a `extras` folder in the
// `$LOCAL_BUILD/include/ode/` which should contain the following header files:
// array.h, collision_kernel.h, common.h, error.h, objects.h, odeou.h, odetls.h, threading_base.h, and typedefs.h.
// These header files can be found in the `ode/src` folder (like here: https://github.com/thomasmarsh/ODE/tree/master/ode/src)
//
// Why do we need to do that? The reason is that for `raisim::Mesh`, the authors of RaiSim use the `dSpaceID` variable
// type which has been forward declared in `ode/common.h` (but not defined there) as such:
//
// struct dxSpace;
// typedef struct dxSpace *dSpaceID;
//
// Thus for `dSpaceID` we need the definition of `dxSpace`, and this one is defined in `ode/src/collision_kernel.h` (in
// the `src` folder and not in the `include` folder!!). Pybind11 is looking for that definition, if you don't include
// it, pybind11 will complain and raise errors.
#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) {
/******************/
/* GyroscopicMode */
/******************/
py::enum_<raisim::GyroscopicMode>(m, "GyroscopicMode", py::arithmetic())
.value("IMPLICIT_GYROSCOPIC_FORCE_BODY", raisim::GyroscopicMode::IMPLICIT_GYROSCOPIC_FORCE_BODY) // implicit body model (stable, more computation)
.value("IMPLICIT_GYROSCOPIC_FORCE_WORLD", raisim::GyroscopicMode::IMPLICIT_GYROSCOPIC_FORCE_WORLD) // implicit world model (stable, more computation)
.value("EXPLICIT_GYROSCOPIC_FORCE", raisim::GyroscopicMode::EXPLICIT_GYROSCOPIC_FORCE) // explicit model (unstable, less computation)
.value("NO_GYROSCOPIC_FORCE", raisim::GyroscopicMode::NO_GYROSCOPIC_FORCE);
/********************/
/* SingleBodyObject */
/********************/
py::class_<raisim::SingleBodyObject, raisim::Object>(m, "SingleBodyObject", "Raisim Single Object from which all single objects/bodies (such as box, sphere, etc) inherit from.")
.def(py::init<raisim::ObjectType>(), "Initialize the Object.", py::arg("object_type"))
.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_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")
// this is the same as get_position
.def("get_world_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")
// this is the same as get_rotation_matrix
.def("get_world_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 &self, py::array_t<double> gravity) {
Vec<3> g = convert_np_to_vec<3>(gravity);
return self.getPotentialEnergy(g);
}, R"mydelimiter(
Get the body's potential energy due to gravity.
Args:
gravity (np.array[float[3]]): gravity vector.
Returns:
float: potential energy.
)mydelimiter",
py::arg("gravity"))
.def("get_energy", [](raisim::SingleBodyObject &self, py::array_t<double> gravity) {
Vec<3> g = convert_np_to_vec<3>(gravity);
return self.getEnergy(g);
}, R"mydelimiter(
Get the body's total energy.
Args:
gravity (np.array[float[3]]): gravity vector.
Returns:
float: total energy.
)mydelimiter",
py::arg("gravity"))
.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.
Args:
local_idx (int): local index.
Returns:
float: mass (kg).
)mydelimiter",
py::arg("local_idx"))
.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")
.def("get_collision_object", &raisim::SingleBodyObject::getCollisionObject, R"mydelimiter(
Get the collision object.
Returns:
dGeomID: collision object.
)mydelimiter")
.def("get_gyroscopic_mode", &raisim::SingleBodyObject::getCollisionObject, R"mydelimiter(
Get the gyroscopic mode.
Returns:
raisim.GyroscopicMode: gyroscopic mode (between ['IMPLICIT_GYROSCOPIC_FORCE_BODY',
'IMPLICIT_GYROSCOPIC_FORCE_WORLD', 'EXPLICIT_GYROSCOPIC_FORCE', 'NO_GYROSCOPIC_FORCE'])
)mydelimiter")
.def("set_position", py::overload_cast<const Eigen::Vector3d &>(&raisim::SingleBodyObject::setPosition), R"mydelimiter(
Set the specified origin position.
Args:
origin_position (np.array[float[3]]): origin position.
)mydelimiter",
py::arg("origin_position"))
.def("set_position", py::overload_cast<double, double, double>(&raisim::SingleBodyObject::setPosition), R"mydelimiter(
Set the specified origin position.
Args:
x (float): x component of the origin position.
y (float): y component of the origin position.
z (float): z component of the origin position.
)mydelimiter",
py::arg("x"), py::arg("y"), py::arg("z"))
.def("set_orientation", py::overload_cast<double, double, double, double>(&raisim::SingleBodyObject::setOrientation), R"mydelimiter(
Set the specified orientation (expressed as a quaternion [x,y,z,w]) for the body.
Args:
w (float): scalar component of the quaternion.
x (float): x component of the vector in the quaternion.
y (float): y component of the vector in the quaternion.
z (float): z component of the vector in the quaternion.
)mydelimiter",
py::arg("w")=1.0, py::arg("x")=0.0, py::arg("y")=0.0, py::arg("z")=0.0)
.def("set_orientation", py::overload_cast<const Eigen::Matrix3d &>(&raisim::SingleBodyObject::setOrientation), R"mydelimiter(
Set the specified orientation (expressed as a rotation matrix) for the body.
Args:
rotation_matrix (np.array[float[3,3]]): rotation matrix.
)mydelimiter",
py::arg("rotation_matrix"))
.def("set_orientation", [](raisim::SingleBodyObject &self, py::array_t<double> quaternion) {
Eigen::Quaterniond quat = convert_np_to_quaternion(quaternion);
self.setOrientation(quat);
}, R"mydelimiter(
Set the specified orientation (expressed as a quaternion [x,y,z,w]) for the body.
Args:
quaternion (np.array[float[4]]): quaternion [x,y,z,w].
)mydelimiter",
py::arg("quaternion"))
.def("set_pose", py::overload_cast<const Eigen::Vector3d &, const Eigen::Matrix3d &>(&raisim::SingleBodyObject::setPose), R"mydelimiter(
Set the specified pose for the body.
Args:
position (np.array[float[3]]): origin position vector.
rotation_matrix (np.array[float[3,3]]): rotation matrix.
)mydelimiter",
py::arg("position"), py::arg("rotation_matrix"))
.def("set_pose", [](raisim::SingleBodyObject &self, Eigen::Vector3d position, py::array_t<double> quaternion) {
Eigen::Quaterniond quat = convert_np_to_quaternion(quaternion);
self.setPose(position, quat);
}, R"mydelimiter(
Set the specified pose for the body.
Args:
position (np.array[float[3]]): origin position vector.
quaternion (np.array[float[4]]): quaternion (expressed as [w,x,y,z]).
)mydelimiter",
py::arg("position"), py::arg("quaternion"))
.def("set_velocity", py::overload_cast<const Eigen::Vector3d &, const Eigen::Vector3d &>(&raisim::SingleBodyObject::setVelocity), R"mydelimiter(
Set the specified linear and angular velocities for the body.
Args:
linear_velocity (np.array[float[3]]): linear velocity.
angular_velocity (np.array[float[3,3]]): angular velocity.
)mydelimiter",
py::arg("linear_velocity"), py::arg("angular_velocity"))
.def("set_velocity", py::overload_cast<double, double, double, double, double, double>(&raisim::SingleBodyObject::setVelocity), R"mydelimiter(
Set the specified linear and angular velocities for the body.
Args:
dx (float): x component of the linear velocity.
dy (float): y component of the linear velocity.
dz (float): z component of the linear velocity.
wx (float): x component of the angular velocity.
wy (float): y component of the angular velocity.
wz (float): z component of the angular velocity.
)mydelimiter",
py::arg("dx"), py::arg("dy"), py::arg("dz"), py::arg("wx"), py::arg("wy"), py::arg("wz"))
.def("set_external_force", [](raisim::SingleBodyObject &self, size_t local_idx, py::array_t<double> force) {
// convert np.array[3] to Vec<3>
Vec<3> f = convert_np_to_vec<3>(force);
self.setExternalForce(local_idx, f);
}, R"mydelimiter(
Set the external force on the body.
Args:
local_idx (int): local index.
force (np.array[float[3]]): force vector.
)mydelimiter",
py::arg("local_idx"), py::arg("force"))
.def("set_external_torque", [](raisim::SingleBodyObject &self, size_t local_idx, py::array_t<double> torque) {
// convert np.array[3] to Vec<3>
Vec<3> t = convert_np_to_vec<3>(torque);
self.setExternalTorque(local_idx, t);
}, R"mydelimiter(
Set the external force on the body.
Args:
local_idx (int): local index.
force (np.array[float[3]]): force vector.
)mydelimiter",
py::arg("local_idx"), py::arg("torque"))
.def("set_gyroscopic_mode", &raisim::SingleBodyObject::setGyroscopicMode, R"mydelimiter(
Set the gyroscopic mode for the body.
Args:
mode (GyroscopicMode): gyroscopic mode (select between (between [GyroscopicMode.IMPLICIT_GYROSCOPIC_FORCE_BODY,
GyroscopicMode.IMPLICIT_GYROSCOPIC_FORCE_WORLD, GyroscopicMode.EXPLICIT_GYROSCOPIC_FORCE,
GyroscopicMode.NO_GYROSCOPIC_FORCE])
)mydelimiter",
py::arg("mode"))
.def("prec_contact_solver_update1", [](raisim::Object &self, py::array_t<double> gravity, double dt) {
// convert np.array[3] to Vec<3>
Vec<3> vec = convert_np_to_vec<3>(gravity);
self.preContactSolverUpdate1(vec, dt);
}, py::arg("gravity"), py::arg("dt"))
.def("prec_contact_solver_update2", [](raisim::Object &self, py::array_t<double> gravity, double dt) {
// convert np.array[3] to Vec<3>
Vec<3> vec = convert_np_to_vec<3>(gravity);
self.preContactSolverUpdate1(vec, dt);
}, py::arg("gravity"), py::arg("dt"))
.def("integrate", &raisim::SingleBodyObject::integrate, "integrate.", py::arg("dt"))
.def("get_contact_point_velocity", [](raisim::Object &self, size_t point_id) {
Vec<3> vel;
self.getContactPointVel(point_id, vel);
return convert_vec_to_np(vel);
})
.def("update_collision", &raisim::SingleBodyObject::updateCollision, "Update the collisions.")
.def("set_linear_damping", &raisim::SingleBodyObject::setLinearDamping, R"mydelimiter(
Set the body's linear damping coefficient.
Args:
damping (float): linear damping coefficient.
)mydelimiter",
py::arg("damping"))
.def("set_angular_damping", [](raisim::SingleBodyObject &self, py::array_t<double> damping) {
// convert np.array[3] to Vec<3>
Vec<3> vec = convert_np_to_vec<3>(damping);
self.setAngularDamping(vec);
}, R"mydelimiter(
Set the body's angular damping.
Args:
damping (np.array[float[3]]): angular damping.
)mydelimiter",
py::arg("damping"))
.def("set_body_type", &raisim::SingleBodyObject::setBodyType, R"mydelimiter(
Set the body's type.
Args:
body_type (BodyType): body type.
)mydelimiter",
py::arg("body_type"))
.def("get_collision_group", &raisim::SingleBodyObject::getCollisionGroup, R"mydelimiter(
Get the body's collision group.
Returns:
int: collision group.
)mydelimiter")
.def("get_collision_mask", &raisim::SingleBodyObject::getCollisionMask, R"mydelimiter(
Get the body's collision mask.
Returns:
int: collision mask.
)mydelimiter")
;
/*******/
/* Box */
/*******/
py::class_<raisim::Box, raisim::SingleBodyObject>(m, "Box", "Raisim Box.")
.def(py::init<double, double, double, double>(),
"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_<raisim::Capsule, raisim::SingleBodyObject>(m, "Capsule", "Raisim Capsule.")
.def(py::init<double, double, double>(),
"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 */
/************/
py::class_<raisim::Compound, raisim::SingleBodyObject> compound(m, "Compound", "Raisim Compound bodies.");
py::class_<raisim::Compound::CompoundObjectChild>(compound, "CompoundObjectChild", "Raisim Compound object child.")
.def_readwrite("object_type", &raisim::Compound::CompoundObjectChild::objectType)
.def_property("object_param",
[](raisim::Compound::CompoundObjectChild &self) { // getter
// convert from Vec<4> to np.array
return convert_vec_to_np(self.objectParam);
}, [](raisim::Compound::CompoundObjectChild &self, py::array_t<double> param) { // setter
// convert from np.array to Vec<4>
Vec<4> vec = convert_np_to_vec<4>(param);
self.objectParam = vec;
})
.def_readwrite("material", &raisim::Compound::CompoundObjectChild::material)
.def_readwrite("transformation", &raisim::Compound::CompoundObjectChild::trans);
compound.def(py::init([](const std::vector<raisim::Compound::CompoundObjectChild>& children, double mass,
py::array_t<double> inertia) {
// convert np.array to Mat<3,3>
Mat<3, 3> I = convert_np_to_mat<3, 3>(inertia);
return new Compound(children, mass, I);
}), R"mydelimiter(
Initialize the Compound object.
Args:
children (list[CompoundObjectChild]): list of child objects.
mass (float): total mass of the compound object.
inertia (np.array[float[3,3]]): total inertia matrix of the compound object.
)mydelimiter",
py::arg("children"), py::arg("mass"), py::arg("inertia"));
/********/
/* Cone */
/********/
py::class_<raisim::Cone, raisim::SingleBodyObject>(m, "Cone", "Raisim Cone.")
.def(py::init<double, double, double>(),
"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_<raisim::Cylinder, raisim::SingleBodyObject>(m, "Cylinder", "Raisim Cylinder.")
.def(py::init<double, double, double>(),
"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_<raisim::Mesh, raisim::SingleBodyObject>(m, "Mesh", "Raisim Mesh.")
.def(py::init<const std::string&, dSpaceID>(),
"Initialize a Mesh.\n\n"
"Args:\n"
" filename (str): path to the mesh file.\n"
" space (dSpaceID): collision space.",
py::arg("filename"), py::arg("space"))
.def(py::init([](const std::string& filename, dSpaceID space, double mass, py::array_t<double> inertia,
py::array_t<double> com) {
// convert np to Mat<3,3> and Vec<3>
Mat<3, 3> I = convert_np_to_mat<3, 3>(inertia);
Vec<3> pos = convert_np_to_vec<3>(com);
return new Mesh(filename, space, mass, I, pos);
}), R"mydelimiter(
Initialize the Mesh object.
Args:
filename (str): path to the mesh file.
space (dSpaceID): collision space.
mass (float): mass of the mesh object.
inertia (np.array[float[3,3]]): inertia matrix of the mesh object.
com (np.array[float[3]]): center of mass position, around which the inertia matrix is expressed.
)mydelimiter",
py::arg("filename"), py::arg("space"), py::arg("mass"), py::arg("inertia"), py::arg("com"));
/**********/
/* Sphere */
/**********/
py::class_<raisim::Sphere, raisim::SingleBodyObject>(m, "Sphere", "Raisim Sphere.")
.def(py::init<double, double>(),
"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");
}
pyrobolearn/simulators/raisim_wrapper/src/terrain.cpp
-255
View File
@@ -1,255 +0,0 @@
/**
* Python wrappers for raisim.object.terrain using pybind11.
*
* Copyright (c) 2019, kangd and jhwangbo (original C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // 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"
#include "ode/collision.h"
#include "ode/ode.h"
#include "ode/extras/collision_kernel.h"
// Important note: for the above include ("ode/src/collision_kernel.h"), you have to add a `extras` folder in the
// `$LOCAL_BUILD/include/ode/` which should contain the following header files:
// array.h, collision_kernel.h, common.h, error.h, objects.h, odeou.h, odetls.h, threading_base.h, and typedefs.h.
// These header files can be found in the `ode/src` folder (like here: https://github.com/thomasmarsh/ODE/tree/master/ode/src)
//
// Why do we need to do that? The reason is that for `raisim::Mesh`, the authors of RaiSim use the `dSpaceID` variable
// type which has been forward declared in `ode/common.h` (but not defined there) as such:
//
// struct dxSpace;
// typedef struct dxSpace *dSpaceID;
//
// Thus for `dSpaceID` we need the definition of `dxSpace`, and this one is defined in `ode/src/collision_kernel.h` (in
// the `src` folder and not in the `include` folder!!). Pybind11 is looking for that definition, if you don't include
// it, pybind11 will complain and raise errors.
namespace py = pybind11;
using namespace raisim;
void init_terrain(py::module &m) {
/***********/
/* Terrain */
/***********/
py::class_<raisim::TerrainProperties>(m, "TerrainProperties", "Raisim terrain properties")
.def(py::init<>(), "Initialize the terrain properties")
.def(py::init<double, double, double, double, size_t, size_t, size_t, double, double, double, std::uint32_t>(),
"Initialize the terrain properties.\n\n"
"Args:\n"
" frequency (float): frequency.\n"
" x_size (float): the size in the x direction.\n"
" y_size (float): the size in the y direction.\n"
" z_scale (float): z scale.\n"
" x_samples (int): the number of samples in x.\n"
" y_samples (int): the number of samples in y.\n"
" fractal_octaves (int): the number of fractal octaves.\n"
" fractal_lacunarity (float): the lacunarity of fractals.\n"
" fractal_gain (float): fractal gain.\n"
" step_size (float): the step size.\n"
" seed (int): the random seed.",
py::arg("frequency"), py::arg("x_size"), py::arg("y_size"), py::arg("z_scale"), py::arg("x_samples"),
py::arg("y_samples"), py::arg("fractal_octaves"), py::arg("fractal_lacunarity"), py::arg("fractal_gain"),
py::arg("step_size"), py::arg("seed"))
.def_readwrite("frequency", &raisim::TerrainProperties::frequency)
.def_readwrite("x_size", &raisim::TerrainProperties::xSize)
.def_readwrite("y_size", &raisim::TerrainProperties::ySize)
.def_readwrite("z_scale", &raisim::TerrainProperties::zScale)
.def_readwrite("x_samples", &raisim::TerrainProperties::xSamples)
.def_readwrite("y_samples", &raisim::TerrainProperties::ySamples)
.def_readwrite("fractal_octaves", &raisim::TerrainProperties::fractalOctaves)
.def_readwrite("fractal_lacunarity", &raisim::TerrainProperties::fractalLacunarity)
.def_readwrite("fractal_gain", &raisim::TerrainProperties::fractalGain)
.def_readwrite("step_size", &raisim::TerrainProperties::stepSize)
.def_readwrite("seed", &raisim::TerrainProperties::seed);
/**********/
/* Ground */
/**********/
py::class_<raisim::Ground, raisim::SingleBodyObject>(m, "Ground", "Raisim Ground.")
.def(py::init<double>(),
"Initialize a ground instance.\n\n"
"Args:\n"
" height (float): height of the ground.",
py::arg("height"))
.def("get_height", &raisim::Ground::getHeight, R"mydelimiter(
Get the ground's height.
Returns:
float: height of the ground.
)mydelimiter");
/*************/
/* HeightMap */
/*************/
py::class_<raisim::HeightMap, raisim::SingleBodyObject>(m, "HeightMap", "Raisim HeightMap.")
.def(py::init<double, double, const std::string&>(),
"Initialize a HeightMap instance.\n\n"
"Args:\n"
" x_center (float): the x center of the heightmap in the world.\n"
" y_center (float): the y center of the heightmap in the world.\n"
" filename (str): raisim heightmap filename.",
py::arg("x_center"), py::arg("y_center"), py::arg("filename"))
.def(py::init<double, double, const std::string&, double, double, double, double>(),
"Initialize a HeightMap instance.\n\n"
"Args:\n"
" x_center (float): the x center of the heightmap in the world.\n"
" y_center (float): the y center of the heightmap in the world.\n"
" filename (str): filename to the PNG.\n"
" x_size (float): the size in the x direction.\n"
" y_size (float): the size in the y direction.\n"
" height_scale (float): the height scale.\n"
" height_offset (float): the height offset.",
py::arg("x_center"), py::arg("y_center"), py::arg("filename"), py::arg("x_size"), py::arg("y_size"),
py::arg("height_scale"), py::arg("height_offset"))
.def(py::init<int, int, double, double, double, double, const std::vector<double> &>(),
"Initialize a HeightMap instance.\n\n"
"Args:\n"
" x_samples (int): the number of samples in x.\n"
" y_samples (int): the number of samples in y.\n"
" x_scale (float): the scale in the x direction.\n"
" y_scale (float): the scale in the y direction.\n"
" x_center (float): the x center of the heightmap in the world.\n"
" y_center (float): the y center of the heightmap in the world.\n"
" heights (list[float]): list of desired heights.",
py::arg("x_samples"), py::arg("y_samples"), py::arg("x_scale"), py::arg("y_scale"), py::arg("x_center"),
py::arg("y_center"), py::arg("heights"))
.def(py::init<double, double, const raisim::TerrainProperties &>(),
"Initialize a HeightMap instance.\n\n"
"Args:\n"
" x_center (float): the x center of the heightmap in the world.\n"
" y_center (float): the y center of the heightmap in the world.\n"
" terrain_properties (TerrainProperties): the terrain properties.\n",
py::arg("x_center"), py::arg("y_center"), py::arg("terrain_properties"))
.def("init", &raisim::HeightMap::init, R"mydelimiter(
Initialize the heightmap.
Args:
x_samples (int): the number of samples in x.
y_samples (int): the number of samples in y.
x_scale (float): the scale in the x direction.
y_scale (float): the scale in the y direction.
x_center (float): the x center of the heightmap in the world.
y_center (float): the y center of the heightmap in the world.
)mydelimiter",
py::arg("x_samples"), py::arg("y_samples"), py::arg("x_scale"), py::arg("y_scale"), py::arg("x_center"),
py::arg("y_center"))
.def("get_height_map", &raisim::HeightMap::getHeightMap, R"mydelimiter(
Get the height map.
Returns:
list[float]: height map.
)mydelimiter")
.def("get_height", &raisim::HeightMap::getHeight, R"mydelimiter(
Get the height at the given location.
Args:
x (float): x position.
y (float): y position.
)mydelimiter",
py::arg("x"), py::arg("y"))
.def("destroy_collision_bodies", &raisim::HeightMap::destroyCollisionBodies, R"mydelimiter(
Destroy the collision bodies of the specified collision space.
Args:
id_ (dSpaceID): collision space id.
)mydelimiter",
py::arg("id_"))
.def("get_x_samples", &raisim::HeightMap::getXSamples, R"mydelimiter(
Get the x samples.
Returns:
int: the number of x samples.
)mydelimiter")
.def("get_y_samples", &raisim::HeightMap::getYSamples, R"mydelimiter(
Get the y samples.
Returns:
int: the number of y samples.
)mydelimiter")
.def("get_x_size", &raisim::HeightMap::getXSize, R"mydelimiter(
Get the x size.
Returns:
float: the size in x.
)mydelimiter")
.def("get_y_size", &raisim::HeightMap::getYSize, R"mydelimiter(
Get the y size.
Returns:
float: the size in y.
)mydelimiter")
.def("get_x_center", &raisim::HeightMap::getCenterX, R"mydelimiter(
Get the x center.
Returns:
float: the x center.
)mydelimiter")
.def("get_y_center", &raisim::HeightMap::getCenterY, R"mydelimiter(
Get the y center.
Returns:
float: the y center.
)mydelimiter")
.def("get_height_vector", &raisim::HeightMap::getHeightVector, R"mydelimiter(
Get the height vector.
Returns:
list[float]: the height vector.
)mydelimiter");
}
pyrobolearn/simulators/raisim_wrapper/src/visualizer.cpp
-454
View File
@@ -1,454 +0,0 @@
/**
* Python wrappers for raisimOgre using pybind11.
*
* Copyright (c) 2019, jhwangbo (original C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
#include <iostream>
// include Ogre related headers
#include "Ogre.h"
#include "OgreApplicationContext.h"
#include "OgreInput.h"
// include Raisim Ogre related headers
#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc.
#include "raisim/interfaceClasses.hpp"
#include "raisim/OgreVis.hpp"
//#include "visualizer/helper.hpp"
//#include "visualizer/guiState.hpp"
#include "visualizer/raisimBasicImguiPanel.hpp" // for `imguiSetupCallback` and `imguiRenderCallBack`
#include "visualizer/raisimKeyboardCallback.hpp" // for `raisimKeyboardCallback`
#include "visualizer/visSetupCallback.hpp" // for `setupCallback`
// include headers that allows to convert between raisim::Vec, raisim::Mat, raisim::VecDyn, raisim::Mat to np.arrays.
#include "converter.hpp"
namespace py = pybind11;
using namespace raisim;
/// \brief: Visualizer class which inherits from raisim::OgreVis. It adds other few functionalities that were missing.
//class Visualizer : public raisim::OgreVis {
//
// public:
//
// using MouseButtonCallback = std::function<bool(const MouseButtonEvent &)>;
// using MouseMotionCallback = std::function<bool(const MouseMotionEvent &)>;
// using MouseWheelCallback = std::function<bool(const MouseWheelEvent &)>;
//
// /** set mouse callback. This callback is called for every mouse event */
// void setMouseButtonCallback(MouseButtonCallback callback) { mouseButtonCallback_ = callback; }
// void setMouseMotionCallback(MouseMotionCallback callback) { mouseMotionCallback_ = callback; }
// void setMouseWheelCallback(MouseWheelCallback callback) { mouseWheelCallback_ = callback; }
//
// private:
//
// MouseButtonCallback mouseButtonCallback_ = nullptr;
// MouseMotionCallback mouseMotionCallback_ = nullptr;
// MouseWheelCallback mouseWheelCallback_ = nullptr;
//};
void init_visualizer(py::module &m) {
// create submodule
py::module visualizer_module = m.def_submodule("visualizer", "RaiSim visualizer submodule.");
/*****************/
/* GraphicObject */
/*****************/
py::class_<raisim::GraphicObject>(visualizer_module, "GraphicObject", "Graphic object represents the underlying object.")
.def(py::init<>(), "Instantiate the Graphic Object by setting its orientation, scale, and offset position.");
/****************/
/* VisualObject */
/****************/
py::class_<raisim::VisualObject>(visualizer_module, "VisualObject", "Visual object is for visualization only")
.def(py::init<>(), "Instantiate a visual object (by setting its orientation).");
/***********/
/* OgreVis */
/***********/
// py::nodedelete is because the destructor is non-public (it is private because of Singleton pattern)
py::class_<raisim::OgreVis, std::unique_ptr<raisim::OgreVis, py::nodelete>> ogre_vis(visualizer_module, "OgreVis", "Raisim Ogre visualizer.");
py::enum_<raisim::OgreVis::VisualizationGroup>(ogre_vis, "VisualizationGroup")
.value("RAISIM_OBJECT_GROUP", raisim::OgreVis::VisualizationGroup::RAISIM_OBJECT_GROUP)
.value("RAISIM_COLLISION_BODY_GROUP", raisim::OgreVis::VisualizationGroup::RAISIM_COLLISION_BODY_GROUP)
.value("RAISIM_CONTACT_POINT_GROUP", raisim::OgreVis::VisualizationGroup::RAISIM_CONTACT_POINT_GROUP)
.value("RAISIM_CONTACT_FORCE_GROUP", raisim::OgreVis::VisualizationGroup::RAISIM_CONTACT_FORCE_GROUP);
ogre_vis.def(py::init([](raisim::World *world, uint32_t width=1280, uint32_t height=720,
double fps=60, int anti_aliasing=2) {
// get reference to the Ogre visualizer
auto vis = raisim::OgreVis::get();
// initialize (need to be called before initApp)
vis->setWorld(world);
vis->setWindowSize(width, height);
vis->setImguiSetupCallback(imguiSetupCallback);
vis->setImguiRenderCallback(imguiRenderCallBack);
vis->setKeyboardCallback(raisimKeyboardCallback);
vis->setSetUpCallback(setupCallback);
vis->setAntiAliasing(anti_aliasing);
// starts visualizer thread (this will call `setup()`)
vis->initApp();
// set desired FPS
vis->setDesiredFPS(fps);
// return the visualizer
return vis;
}), R"mydelimiter(
Instantiate the visualizer for the given world.
Args:
world (World): world instance.
width (int): width of the window.
height (int): height of the window.
fps (double): the number of frames per second.
anti_aliasing (int): anti aliasing.
)mydelimiter",
py::arg("world"), py::arg("width") = 1280, py::arg("height") = 720, py::arg("fps") = 60,
py::arg("anti_aliasing") = 2)
.def("get", &raisim::OgreVis::get, R"mydelimiter(
Return a pointer to the singleton visualizer.
Returns:
OgreVis: reference to this class.
)mydelimiter")
.def("create_graphical_object", py::overload_cast<raisim::Sphere*, const std::string&,
const std::string&>(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add a sphere in the window.
Args:
sphere (Sphere): Raisim sphere instance.
name (str): name of the sphere.
material (str): material for visualization.
)mydelimiter",
py::arg("sphere"), py::arg("name"), py::arg("material"))
.def("create_graphical_object", py::overload_cast<raisim::Ground *, double, const std::string&,
const std::string&>(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add a ground in the window.
Args:
ground (Ground): Raisim ground instance.
dimension (double): the plane dimension.
name (str): name of the ground.
material (str): material for visualization.
)mydelimiter",
py::arg("ground"), py::arg("dimension"), py::arg("name"), py::arg("material"))
.def("create_graphical_object", py::overload_cast<raisim::Box*, const std::string&,
const std::string&>(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add a box in the window.
Args:
box (Box): Raisim box instance.
name (str): name of the box.
material (str): material for visualization.
)mydelimiter",
py::arg("box"), py::arg("name"), py::arg("material"))
.def("create_graphical_object", py::overload_cast<raisim::Cylinder*, const std::string&,
const std::string&>(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add a cylinder in the window.
Args:
cylinder (Cylinder): Raisim cylinder instance.
name (str): name of the cylinder.
material (str): material for visualization.
)mydelimiter",
py::arg("cylinder"), py::arg("name"), py::arg("material"))
.def("create_graphical_object", py::overload_cast<raisim::Wire*, const std::string&,
const std::string&>(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add a wire in the window.
Args:
wire (Wire): Raisim wire instance.
name (str): name of the wire.
material (str): material for visualization.
)mydelimiter",
py::arg("wire"), py::arg("name"), py::arg("material") = "default")
.def("create_graphical_object", py::overload_cast<raisim::Capsule*, const std::string&,
const std::string&>(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add a capsule in the window.
Args:
capsule (Capsule): Raisim capsule instance.
name (str): name of the capsule.
material (str): material for visualization.
)mydelimiter",
py::arg("capsule"), py::arg("name"), py::arg("material"))
.def("create_graphical_object", py::overload_cast<raisim::ArticulatedSystem*, const std::string&>
(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add an articulated system in the window.
Args:
articulated_system (ArticulatedSystem): Raisim articulated system instance.
name (str): name of the articulated system.
)mydelimiter",
py::arg("articulated_system"), py::arg("name"))
.def("create_graphical_object", py::overload_cast<raisim::HeightMap*, const std::string&,
const std::string&>(&raisim::OgreVis::createGraphicalObject), R"mydelimiter(
Add a heightmap in the window.
Args:
heightmap (HeightMap): Raisim heightmap instance.
name (str): name of the heightmap.
material (str): material for visualization.
)mydelimiter",
py::arg("capsule"), py::arg("name"), py::arg("material") = "default")
.def("sync", &raisim::OgreVis::sync, "Synchronize Raisim and Ogre.")
.def("get_paused", &raisim::OgreVis::getPaused, R"mydelimiter(
Return if the visualizer is paused or not.
Returns:
bool: True if the visualizer is paused.
)mydelimiter")
.def_property_readonly("paused", &raisim::OgreVis::getPaused, "Return if the visualizer is paused or not.")
.def("remove", py::overload_cast<raisim::Object*>(&raisim::OgreVis::remove), R"mydelimiter(
Remove an object from the visualizer.
Args:
obj (Object): Raisim object instance to be removed.
)mydelimiter",
py::arg("obj"))
.def("remove", py::overload_cast<const std::string&>(&raisim::OgreVis::remove), R"mydelimiter(
Remove an object from the visualizer.
Args:
name (str): name of the object to be removed.
)mydelimiter",
py::arg("name"))
.def("get_selected", &raisim::OgreVis::getSelected, R"mydelimiter(
Return the current selected item.
Returns:
Object: Raisim object instance.
int: index.
)mydelimiter")
.def("get_selected_graphical_object", &raisim::OgreVis::getSelectedGraphicalObject, R"mydelimiter(
Return the current selected graphical object item.
Returns:
GraphicObject: Raisim graphic object instance.
)mydelimiter")
.def("select", &raisim::OgreVis::select, R"mydelimiter(
Select the given graphic object item.
Args:
obj (GraphicObject): Raisim graphic object instance.
highlight (bool): if we should highlight the graphical object in the visualizer.
)mydelimiter",
py::arg("obj"), py::arg("highlight") = true)
.def("deselect", &raisim::OgreVis::deselect, "Deselect the current selected object.")
// TODO: wrap Ogre::SceneNode
// .def("get_raisim_object", &raisim::OgreVis::getRaisimObject, "get Raisim object.")
// TODO: wrap Ogre::SceneNode
// .def("get_graphic_object", &raisim::OgreVis::getGraphicObject, "get the graphic object.")
.def("is_recording", &raisim::OgreVis::isRecording, R"mydelimiter(
Return if the visualizer is recording or not.
Returns:
bool: True if the visualizer is recording.
)mydelimiter")
.def("start_recording_video", &raisim::OgreVis::startRecordingVideo, R"mydelimiter(
Initiate a video recording session.
Returns:
filename (str): filename for the recorded video.
)mydelimiter",
py::arg("filename"))
.def("stop_recording_video_and_save", &raisim::OgreVis::stopRecordingVideoAndSave,
"Stop the recording of the video and save it in the previous given filename.")
.def("set_desired_fps", &raisim::OgreVis::setDesiredFPS, R"mydelimiter(
Set the desired frame per second.
Args:
fps (double): frame per second.
)mydelimiter",
py::arg("fps"))
.def("set_visibility_mask", &raisim::OgreVis::setVisibilityMask, R"mydelimiter(
Set the visibility mask.
Args:
mask (unsigned long int): mask (it is a bitfield).
)mydelimiter",
py::arg("mask"))
.def("get_visual_object_list", &raisim::OgreVis::getVisualObjectList, R"mydelimiter(
Return the list of visual objects.
Returns:
dict[str:VisualObject]: dictionary mapping names to visual objects.
)mydelimiter")
.def("set_contact_visual_object_size", &raisim::OgreVis::setContactVisObjectSize, R"mydelimiter(
Set the contact visual object size.
Args:
point_size (float): point size.
force_arrow_length (float): force size corresponding to the maximum impulse.
)mydelimiter",
py::arg("point_size"), py::arg("force_arrow_length"))
.def("get_real_time_factor_reference", &raisim::OgreVis::getRealTimeFactorReference, R"mydelimiter(
Get the real time factor reference.
Returns:
float: real time factor reference.
)mydelimiter")
.def("set_remote_mode", &raisim::OgreVis::setRemoteMode, R"mydelimiter(
Set the remote mode.
Args:
mode (bool): True if we are in a remote mode.
)mydelimiter",
py::arg("mode"))
.def("remote_run", &raisim::OgreVis::remoteRun, "Run in remote mode.")
.def("add_visual_object", [](raisim::OgreVis &self, const std::string &name, const std::string &mesh_name,
const std::string &material, py::array_t<double> scale, bool cast_shadow = true,
unsigned long int group = raisim::OgreVis::VisualizationGroup::RAISIM_OBJECT_GROUP |
raisim::OgreVis::VisualizationGroup::RAISIM_COLLISION_BODY_GROUP) {
// convert np.array to vec<3>
raisim::Vec<3> scale_ = convert_np_to_vec<3>(scale);
self.addVisualObject(name, mesh_name, material, scale_, cast_shadow, group);
}, R"mydelimiter(
Add a visual object.
Args:
name (str): name of the visual object.
mesh_name (str): name of the material.
material (str): material.
scale (np.array[float[3]]): scale.
cast_shadow (bool): if we should cast shadow or not.
group (unsigned long int): group. You can select between {RAISIM_OBJECT_GROUP, RAISIM_COLLISION_BODY_GROUP,
RAISIM_CONTACT_POINT_GROUP, RAISIM_CONTACT_FORCE_GROUP}, or any combination using bit operations.
)mydelimiter",
py::arg("name"), py::arg("mesh_name"), py::arg("material"), py::arg("scale"), py::arg("cast_shadow") = true,
py::arg("group") = raisim::OgreVis::VisualizationGroup::RAISIM_OBJECT_GROUP |
raisim::OgreVis::VisualizationGroup::RAISIM_COLLISION_BODY_GROUP)
.def("clear_visual_object", &raisim::OgreVis::clearVisualObject, "Clear all the visual objects.")
.def("build_height_map", &raisim::OgreVis::buildHeightMap, R"mydelimiter(
Build the heigthmap.
Args:
name (str): the heightmap name.
x_samples (int): the number of samples in x.
x_size (float): the size in the x direction.
x_center (float): the x center of the heightmap in the world.
y_samples (int): the number of samples in y.
y_size (float): the size in the y direction.
y_center (float): the y center of the heightmap in the world.
height (list[float]): list of desired heights.
)mydelimiter",
py::arg("name"), py::arg("x_samples"), py::arg("x_size"), py::arg("x_center"), py::arg("y_samples"),
py::arg("y_size"), py::arg("y_center"), py::arg("height"))
;
// py::class_<Visualizer, raisim::OgreVis> visualizer(visualizer_module, "Visualizer", "The visualizer class allows you to create a "
// "window from which you can visualize the simulation.");
// py::class_<Visualizer, raisim::OgreVis> visualizer(m, "Visualizer", "Visualizer");
// visualizer.def("__init__", [](Visualizer &self, uint32_t width, uint32_t height, double fps=60) {
// // initialize
//
// // starts visualizer thread (this will call `setup()`)
// self.initApp();
//
// // set desired FPS
// self.setDesiredFPS(fps)
// }, "Instantiate the visualizer.")
// .def("get", &Visualizer::get, "Return a pointer to the singleton visualizer.");
//// .def("create_graphical_object", [](Visualizer &self, const std::string &name, const std::string &meshname,
//// const std::string &material, const py::array_t<double> &scale, const py::array_t<double> &offset,
//// const py::array_t<double> &rotation_matrix, size_t local_idx, bool cast_shadow=true, bool selectable=true,
//// unsigned long int group = ));
}
pyrobolearn/simulators/raisim_wrapper/src/world.cpp
-560
View File
@@ -1,560 +0,0 @@
/**
* Python wrappers for raisim.World using pybind11.
*
* Copyright (c) 2019, kangd (original C++), Brian Delhaisse <briandelhaisse@gmail.com> (Python wrappers)
*
* 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 <pybind11/pybind11.h>
#include <pybind11/stl.h> // automatic conversion between std::vector, std::list, etc to Python list/tuples/dict
#include <pybind11/eigen.h> // automatic conversion between Eigen data types to Numpy data types
#include <iostream>
#include "raisim/math.hpp" // contains the definitions of Vec, Mat, etc.
#include "raisim/World.hpp"
#include "raisim/RaisimServer.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_world(py::module &m) {
/*********/
/* World */
/*********/
py::class_<raisim::World>(m, "World", "Raisim world.", py::dynamic_attr()) // enable dynamic attributes for C++ class in Python
.def(py::init<>(), "Initialize the World.")
.def(py::init<const std::string &>(), "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", py::overload_cast<int, int, double, double, double, double, const std::vector<double> &,
const std::string &, CollisionGroup, CollisionGroup>(&raisim::World::addHeightMap), R"mydelimiter(
Add a heightmap into the world.
Args:
x_samples (int): the number of samples in x.
y_samples (int): the number of samples in y.
x_scale (float): the scale in the x direction.
y_scale (float): the scale in the y direction.
x_center (float): the x center of the heightmap in the world.
y_center (float): the y center of the heightmap in the world.
heights (list[float]): list of desired heights.
material (str): material.
group (CollisionGroup): collision group.
mask (CollisionGroup): collision mask.
Returns:
HeightMap: the heightmap instance.
)mydelimiter",
py::arg("x_samples"), py::arg("y_samples"), py::arg("x_scale"), py::arg("y_scale"), py::arg("x_center"),
py::arg("y_center"), py::arg("heights"), py::arg("material") = "default", py::arg("collision_group") = 1,
py::arg("collision_mask") = CollisionGroup(-1))
.def("add_heightmap", py::overload_cast<const std::string &, double, double, const std::string &,
CollisionGroup, CollisionGroup>(&raisim::World::addHeightMap), R"mydelimiter(
Add a heightmap into the world.
Args:
filename (str): raisim heightmap filename.
x_center (float): the x center of the heightmap in the world.
y_center (float): the y center of the heightmap in the world.
material (str): material.
group (CollisionGroup): collision group.
mask (CollisionGroup): collision mask.
Returns:
HeightMap: the heightmap instance.
)mydelimiter",
py::arg("filename"), py::arg("x_center"), py::arg("y_center"), py::arg("material") = "default",
py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1))
.def("add_heightmap", py::overload_cast<const std::string &, double, double, double, double, double, double,
const std::string &, CollisionGroup, CollisionGroup>(&raisim::World::addHeightMap), R"mydelimiter(
Add a heightmap into the world.
Args:
filename (str): filename to the PNG.
x_center (float): the x center of the heightmap in the world.
y_center (float): the y center of the heightmap in the world.
x_size (float): the size in the x direction.
y_size (float): the size in the y direction.
height_scale (float): the height scale.
height_offset (float): the height offset.
material (str): material.
group (CollisionGroup): collision group.
mask (CollisionGroup): collision mask.
Returns:
HeightMap: the heightmap instance.
)mydelimiter",
py::arg("filename"), py::arg("x_center"), py::arg("y_center"), py::arg("x_size"), py::arg("y_size"),
py::arg("height_scale"), py::arg("height_offset"), py::arg("material") = "default",
py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1))
.def("add_heightmap", py::overload_cast<double, double, raisim::TerrainProperties&, const std::string &,
CollisionGroup, CollisionGroup>(&raisim::World::addHeightMap), R"mydelimiter(
Add a heightmap into the world.
Args:
x_center (float): the x center of the heightmap in the world.
y_center (float): the y center of the heightmap in the world.
terrain_properties (TerrainProperties): the terrain properties.
material (str): material.
group (CollisionGroup): collision group.
mask (CollisionGroup): collision mask.
Returns:
HeightMap: the heightmap instance.
)mydelimiter",
py::arg("x_center"), py::arg("y_center"), py::arg("terrain_properties"),
py::arg("material") = "default", py::arg("collision_group") = 1, py::arg("collision_mask") = CollisionGroup(-1))
.def("add_articulated_system", &raisim::World::addArticulatedSystem, R"mydelimiter(
Add an articulated system in the world.
Args:
urdf_path (str): path to the URDF file.
res_path (str): path to the resource directory. Leave it empty ('') if it is the urdf file directory.
joint_order (list[str]): joint order.
group (CollisionGroup): collision group.
mask (CollisionGroup): collision mask.
options (ArticulatedSystemOption): articulated system options.
Returns:
ArticulatedSystem: the articulated system instance.
)mydelimiter",
py::arg("urdf_path"), py::arg("res_path") = "", py::arg("joint_order") = std::vector<std::string>(), py::arg("collision_group") = 1,
py::arg("collision_mask") = CollisionGroup(-1), py::arg("options") = raisim::ArticulatedSystemOption())
.def("add_compound", [](raisim::World &self, const std::vector<raisim::Compound::CompoundObjectChild> &children,
double mass, py::array_t<double> inertia, CollisionGroup group=1,
CollisionGroup mask = CollisionGroup(-1)) {
// convert np.array to Mat<3,3>
Mat<3, 3> I = convert_np_to_mat<3, 3>(inertia);
// return compound object
return self.addCompound(children, mass, I, group, mask);
}, R"mydelimiter(
Add a compound body in the world.
Args:
children (list[CompoundObjectChild]): list of child object instance.
mass (float): mass of the compound object.
inertia (np.array[float[3,3]]): inertia matrix of the object.
group (CollisionGroup): collision group.
mask (CollisionGroup): collision mask.
Returns:
ArticulatedSystem: the articulated system instance.
)mydelimiter",
py::arg("children"), py::arg("mass") = "", py::arg("inertia"), py::arg("collision_group") = 1,
py::arg("collision_mask") = CollisionGroup(-1))
.def("add_stiff_wire", [](raisim::World &self, raisim::Object &object1, size_t local_idx1,
py::array_t<double> pos_body1, raisim::Object &object2, size_t local_idx2,
py::array_t<double> 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);
// return the stiff wire instance.
return self.addStiffWire(&object1, local_idx1, pos1, &object2, local_idx2, pos2, length);
}, R"mydelimiter(
Add a stiff wire constraint between two bodies in the world.
Args:
object1 (Object): first object/body instance.
local_idx1 (int): local index of the first object/body.
pos_body1 (np.array[float[3]]): position of the constraint on the first body.
object2 (Object): second object/body instance.
local_idx2 (int): local index of the second object/body.
pos_body2 (np.array[float[3]]): position of the constraint on the second body.
length (float): length of the wire constraint.
Returns:
StiffWire: the stiff wire constraint instance.
)mydelimiter",
py::arg("object1"), py::arg("local_idx1"), py::arg("pos_body1"), py::arg("object2"), py::arg("local_idx2"),
py::arg("pos_body2"), py::arg("length"))
.def("add_compliant_wire", [](raisim::World &self, raisim::Object &object1, size_t local_idx1,
py::array_t<double> pos_body1, raisim::Object &object2, size_t local_idx2,
py::array_t<double> 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);
// return the compliant wire instance.
return self.addCompliantWire(&object1, local_idx1, pos1, &object2, local_idx2, pos2, length, stiffness);
}, R"mydelimiter(
Add a compliant wire constraint between two bodies in the world.
Args:
object1 (Object): first object/body instance.
local_idx1 (int): local index of the first object/body.
pos_body1 (np.array[float[3]]): position of the constraint on the first body.
object2 (Object): second object/body instance.
local_idx2 (int): local index of the second object/body.
pos_body2 (np.array[float[3]]): position of the constraint on the second body.
length (float): length of the wire constraint.
stiffness (float): stiffness of the wire.
Returns:
CompliantWire: the compliant wire constraint instance.
)mydelimiter",
py::arg("object1"), py::arg("local_idx1"), py::arg("pos_body1"), py::arg("object2"), py::arg("local_idx2"),
py::arg("pos_body2"), py::arg("length"), py::arg("stiffness"))
.def("get_object", &raisim::World::getObject, R"mydelimiter(
Get the specified object instance from its unique name.
Args:
name (str): unique name of the object instance we want to get.
Returns:
Object, None: the specified object instance. None, if it didn't find the object.
)mydelimiter",
py::arg("name"))
.def("get_constraint", &raisim::World::getConstraint, R"mydelimiter(
Get the specified constraint instance from its unique name.
Args:
name (str): unique name of the constraint instance we want to get.
Returns:
Constraints, None: the specified constraint instance. None, if it didn't find the constraint.
)mydelimiter",
py::arg("name"))
.def("get_wire", &raisim::World::getWire, R"mydelimiter(
Get the specified wire instance from its unique name.
Args:
name (str): unique name of the wire instance we want to get.
Returns:
Constraints: the specified wire instance. None, if it didn't find the wire.
)mydelimiter",
py::arg("name"))
.def("get_configuration_number", &raisim::World::getConfigurationNumber, R"mydelimiter(
Get the number of elements that are in the world. The returned number is updated everytime that we add or
remove an object from the world.
Returns:
int: the number of objects in the world.
)mydelimiter")
.def("remove_object", py::overload_cast<raisim::Object*>(&raisim::World::removeObject), R"mydelimiter(
Remove dynamically an object from the world.
Args:
obj (Object): the object to be removed from the world.
)mydelimiter",
py::arg("obj"))
.def("remove_object", py::overload_cast<raisim::StiffWire*>(&raisim::World::removeObject), R"mydelimiter(
Remove dynamically a stiff wire from the world.
Args:
wire (StiffWire): the stiff wire to be removed from the world.
)mydelimiter",
py::arg("wire"))
.def("remove_object", py::overload_cast<raisim::CompliantWire*>(&raisim::World::removeObject), R"mydelimiter(
Remove dynamically a compliant wire from the world.
Args:
wire (CompliantWire): the compliant wire to be removed from the world.
)mydelimiter",
py::arg("wire"))
.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")
// TODO: improve the doc for the below method
.def("get_contact_problems", &raisim::World::getContactProblem, R"mydelimiter(
Return the list of contacts.
)mydelimiter")
.def("get_object_list", &raisim::World::getObjList, R"mydelimiter(
Return the list of object instances that are in the world.
Returns:
list[Object]: list of object instances.
)mydelimiter")
.def("update_material_property", &raisim::World::updateMaterialProp, R"mydelimiter(
Update material property.
Args:
prop (MaterialManager): material manager property instance.
)mydelimiter",
py::arg("prop"))
.def("set_material_pair_properties", &raisim::World::setMaterialPairProp, R"mydelimiter(
Set material pair properties.
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("set_default_material", &raisim::World::setDefaultMaterial, R"mydelimiter(
Set the default material.
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"))
.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<double> 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"))
.def("get_contact_solver", py::overload_cast<>(&raisim::World::getContactSolver), R"mydelimiter(
Return the bisection contact solver used.
Returns:
BisectionContactSolver: contact solver.
)mydelimiter");
}
pyrobolearn/simulators/raisim_wrapper/test.py
-21
View File
@@ -1,21 +0,0 @@
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))