Commit 943f0927 authored by Timothee Habra's avatar Timothee Habra

add new contact model libraries

parent 8ee63a88
......@@ -8,6 +8,11 @@
#
## ---- GCM choice ---- ##
add_definitions( -DMESH_GSM )
#add_definitions( -DCUBOID_GCM )
## ---- FLAGS ---- ##
......@@ -353,6 +358,7 @@ endfunction()
function(list_include_directories arg0 arg1)
file(GLOB_RECURSE SOURCE_FILES_TMP RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
"${PROJECT_SOURCE_DIR}/${arg1}/*.h"
"${PROJECT_SOURCE_DIR}/${arg1}/*.hh"
"${PROJECT_SOURCE_DIR}/${arg1}/*.hpp")
set(CUR_DIR_LIST "")
foreach(FILE_PATH ${SOURCE_FILES_TMP})
......@@ -380,7 +386,7 @@ function(list_source_files arg0 arg1 arg2)
set(SOURCES ${SOURCES} ${SRC_FILE})
endif (NOT PROHIBITED_FILE)
endforeach(SRC_FILE)
set(${arg0} ${SOURCES} PARENT_SCOPE)
set(${arg0} ${${arg0}} ${SOURCES} PARENT_SCOPE)
endfunction()
# list all files (.c, .cc, .cpp, .h and .hpp)
......@@ -396,6 +402,8 @@ function(list_folder_files arg0 arg1)
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
list_source_files(CUR_LIST "*.h" ${arg1})
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
list_source_files(CUR_LIST "*.hh" ${arg1})
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
list_source_files(CUR_LIST "*.hpp" ${arg1})
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
set(${arg0} ${CUR_FULL_LIST} PARENT_SCOPE)
......@@ -435,7 +443,7 @@ function(list_source_files_2 arg0 arg1 arg2 arg3)
set(CUR_FULL_LIST ${CUR_LIST})
list_source_files(CUR_LIST ${arg1} ${arg3})
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
set(${arg0} ${CUR_FULL_LIST} PARENT_SCOPE)
set(${arg0} ${${arg0}}${CUR_FULL_LIST} PARENT_SCOPE)
endfunction()
# list_include_directories into 3 folders: arg2, arg3 and arg4
......@@ -449,7 +457,7 @@ function(list_source_files_3 arg0 arg1 arg2 arg3 arg4)
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
list_source_files(CUR_LIST ${arg1} ${arg4})
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
set(${arg0} ${CUR_FULL_LIST} PARENT_SCOPE)
set(${arg0} ${${arg0}} ${CUR_FULL_LIST} PARENT_SCOPE)
endfunction()
# list_include_directories into 1 folder (arg3),
......@@ -461,7 +469,7 @@ function(list_source_files_4 arg0 arg1 arg2 arg3)
set(CUR_FULL_LIST ${CUR_LIST})
list_source_files(CUR_LIST ${arg2} ${arg3})
set(CUR_FULL_LIST ${CUR_FULL_LIST} ${CUR_LIST})
set(${arg0} ${CUR_FULL_LIST} PARENT_SCOPE)
set(${arg0} ${${arg0}} ${CUR_FULL_LIST} PARENT_SCOPE)
endfunction()
......
......@@ -6,12 +6,49 @@
#
# list all source files
list_source_files_3(SOURCE_FILES_C "*.c" "../symbolicR" "src/project" "src/generic")
list_source_files_3(SOURCE_FILES_CPP "*.cpp" "../symbolicR" "src/project" "src/generic")
list_source_files(SYMBOLIC_SOURCE_FILES_C "*.c" "../symbolicR")
list_source_files(PROJECT_SOURCE_FILES_C "*.c" "src/project")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/Simbody")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/integrator")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/jni")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/mbs_load_xml")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/post_process")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/real_time")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/robotran")
#list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/robotran_yarp_plugin")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/sdl")
list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/useful")
# list_source_files(GENERIC_SOURCE_FILES_C "*.c" "src/generic/other")
list_source_files(MAIN_SOURCE_FILES_C "*.c" "src/generic/main_files")
set(SOURCE_FILES_C ${SYMBOLIC_SOURCE_FILES_C} ${PROJECT_SOURCE_FILES_C} ${GENERIC_SOURCE_FILES_C} ${SYMBOLIC_SOURCE_FILES_C} ${MAIN_SOURCE_FILES_C} )
list_source_files(SYMBOLIC_SOURCE_FILES_CPP "*.cpp" "../symbolicR")
list_source_files(PROJECT_SOURCE_FILES_CPP "*.cpp" "src/project")
list_source_files(PROJECT_SOURCE_FILES_CC "*.cc" "src/project")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/Simbody")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/integrator")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/jni")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/mbs_load_xml")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/post_process")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/real_time")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/robotran")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/robotran_yarp_plugin")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/sdl")
list_source_files(GENERIC_SOURCE_FILES_CPP "*.cpp" "src/generic/useful")
list_source_files(MAIN_SOURCE_FILES_CPP "*.cpp" "src/generic/main_files")
set(SOURCE_FILES_CPP ${SYMBOLIC_SOURCE_FILES_CPP} ${PROJECT_SOURCE_FILES_CPP} ${PROJECT_SOURCE_FILES_CC} ${GENERIC_SOURCE_FILES_CPP} ${MAIN_SOURCE_FILES_CPP})
# list_source_files_3(SOURCE_FILES_C "*.c" "../symbolicR" "src/project" "src/generic")
# list_source_files_3(SOURCE_FILES_CPP "*.cpp" "../symbolicR" "src/project" "src/generic")
list_source_files_3(HEADER_FILES_H "*.h" "../symbolicR" "src/project" "src/generic")
list_source_files_3(HEADER_FILES_HPP "*.hpp" "../symbolicR" "src/project" "src/generic")
list_source_files_3(HEADER_FILES_HH "*.hh" "../symbolicR" "src/project" "src/generic")
set(HEADER_FILES ${HEADER_FILES_H} ${HEADER_FILES_HPP})
set(HEADER_FILES ${HEADER_FILES_H} ${HEADER_FILES_HPP} ${HEADER_FILES_HH})
# message(INFO "SOURCE_FILES_C is ${SOURCE_FILES_C}")
# message(INFO "SOURCE_FILES_CPP is ${SOURCE_FILES_CPP}")
# list all source files from the 'generic' folder
list_folder_files(SRC_GEN_INT "src/generic/integrator")
......
......@@ -8,6 +8,9 @@
#include "MBSfun.h"
#include <libxml/xmlmemory.h>
#include <libxml/parser.h>
#include <libxml/tree.h>
MBSdataStruct* loadMBSdata_xml(const char *filein);
MBSdataStruct* loadMBSsizes_xml(xmlDocPtr doc, xmlNodePtr cur);
......
......@@ -17,11 +17,6 @@
//#include "mex.h"
//#endif
//------ Custom defines (not in Robotran standard files) -------//
#include <libxml/xmlmemory.h>
#include <libxml/parser.h>
#include <libxml/tree.h>
//--------------------------------------------------------------//
#include "MBSdataStruct.h"
......
......@@ -14,12 +14,12 @@
// ControllerInputsStruc
typedef struct ControllerInputs
{
double t;
double q[29];
double qd[29];
double Qq[29];
double q_mot[29];
double qd_mot[29];
double t; // time [s]
double q[29]; // link position [rad]
double qd[29]; // link velocity [rad/s]
double Qq[29]; // link torque [Nm]
double q_mot[29]; // motor position [rad]
double qd_mot[29];// motor velocity [rad/s]
double F_Rfoot[3];
double F_Lfoot[3];
double T_Rfoot[3];
......@@ -34,9 +34,9 @@ typedef struct ControllerInputs
// ControllerOutputsStruc
typedef struct ControllerOutputs
{
double q_ref[29];
double q_ref[29]; // joint reference position [rad]
double qd_ref[29];
double Qq_ref[29];
double Qq_ref[29]; // joint reference torque [Nm]
int imp_ctrl_index[29];
} ControllerOutputs;
......
/*!
* \author Nicolas Van der Noot
* \file UserShapeModule.cc
* \brief user own class for the shapes
*
* This class can be used by the user to add variables and functions (also in 'UserShapeModule.hh')
* to each shape (union, rigid or basic). This might for instance be used to assign a different
* friction coefficients to the different basic shapes.
*
* To this end, you can for instance use this line in 'user_shapes.cc':
* cur_basic = cur_rigid->add_sphere(0.2, 0.0, 0.0, 0.0);
* where 'cur_basic' is the current BasicShape created and 'cur_rigid' is the current RigidShape.
*
* Then, the function 'UserShapeModule& get_module()' can be used to recover the corresponding
* UserShapeModule. You can for instance use these lines:
* cur_user_module = cur_basic->get_module();
* and then call your own fucntions to set the requested friction coefficient for instance.
*
* Then, to use it, you can for instance go inside 'user_contact_force.cc' or 'user_contact_states.cc'
* and call this line:
* cur_user_module = shape->get_module();
* Then, you can retrieve the shape properties defined in 'user_shapes.cc'.
*/
#include "UserShapeModule.hh"
namespace ContactGeom{
/*! \brief constructor
*/
UserShapeModule::UserShapeModule()
{
}
/*! \brief destructor
*/
UserShapeModule::~UserShapeModule()
{
}
}
/*!
* \author Nicolas Van der Noot
* \file UserShapeModule.hh
* \brief UserShapeModule class
*/
#ifndef _USER_SHAPE_MODULE_HH_
#define _USER_SHAPE_MODULE_HH_
namespace ContactGeom{
/*! \brief user module to personalize shapes properties
*/
class UserShapeModule
{
public:
UserShapeModule();
~UserShapeModule();
private:
};
}
#endif
#include "user_contact_forces.hh"
#include "ContactVolume.hh"
namespace ContactGeom{
// normal contribution
#define KP_N 1.0e10 ///< normal stiffnes coeff [N/(m^3)]
#define KD_N 1.0e2 ///< normal damping contribution [s/(m^3)]
#define MU_MAX 0.9 ///< maximal value for the friction coefficient [-]
#define BETA_TG 100.0 ///< tanh inner factor [-]
/// if negative, return 0, otherwise, return the input as output
inline double pos(double x) { return (x > 0.0) ? x : 0.0; }
/*! \brief get normal force
*
* \param[in] shape basic shape on which the force is exerted
* \param[in] other_shape other basic shape in contact with 'shape'
* \param[in] volume penetration volume [m^3] (always positive)
* \param[in] volume_dot penetration volume derivative [m^3/s]
* \return normal force [N]
*
* The purpose of this function is to compute the normal force (scalar, supposed possitive)
* resulting from the contact between the basic shape 'shape'and another basic shape 'other_shape'.
* To this end, the penetration volume (i.e. intersection volume beween these two shapes) is provided,
* together with its derivative.
*
* You can use 'shape->get_module()' to access this shape own module (defined in 'UserShapeModule.hh').
* In a similar way, you can also use 'other_shape->get_module()' for the other shape.
*/
double normal_force(BasicShape *shape, BasicShape *other_shape, double volume, double volume_dot)
{
return KP_N * volume * pos(1.0 + KD_N * volume_dot);
}
/*! \brief get tangential force norm on a point
*
* \param[in] shape basic shape on which the force is exerted
* \param[in] other_shape other basic shape in contact with 'shape'
* \param[in] v relative tangetial speed on this point (norm, always positive) [m/s]
* \param[in] F_N normal force applied on this point (norm, always positive) [N]
* \return tangential force [N]
*
* The purpose of this function is to compute the tangential force (scalar, supposed positive).
* To this end, the norm of the relative speed beween the two shapes involved ('shape' and 'other_shape')
* is provided, as well as the normal force applied on this contact point.
*
* You can use 'shape->get_module()' to access this shape own module (defined in 'UserShapeModule.hh').
* In a similar way, you can also use 'other_shape->get_module()' for the other shape.
*/
double tangentiel_force(BasicShape *shape, BasicShape *other_shape, double v, double F_N)
{
return F_N * MU_MAX * tanh(BETA_TG * v);
}
}
/*!
* \author Nicolas Van der Noot
* \file user_contact_forces.hh
* \brief user contact force parametrization
*/
#ifndef _USER_CONTACT_FORCES_HH_
#define _USER_CONTACT_FORCES_HH_
#include "BasicShape.hh"
namespace ContactGeom{
// function prototypes
double normal_force(BasicShape *shape, BasicShape *other_shape, double volume, double volume_dot);
double tangentiel_force(BasicShape *shape, BasicShape *other_shape, double v, double F_N);
}
#endif
#include "MainUnionShape.hh"
#include "user_contact_kinematics.hh"
#include "RigidShape.hh"
namespace ContactGeom{
/*! \brief kinematics update
*
* \param[out] kin_info_list kinematic information of all moving bodies
* \param[in] main_union main union of shapes
* \param[in] MBSdata Robotran structure
*
* This function is used to compute the kinematics of all bodies involved in the contact model.
* Using the line 'main_union->mbs_sensor_compute();' is enough to automatically get the correct result.
* Unfortunately, this function can be quite slow because it uses the 'sensor' function, which recomputes
* all the kinematics of each body, each time starting from scratch.
*
* For more advance users who want to improve the computational efficiency, you can remove this line and
* perform yourself the computation. The purpose of this function is to fill 'kin_info_list', as can
* be seen in the 'update_kin_info' function (see bottom of this file).
*
* An easy solution to handle this computation in an efficient way is to do as follows:
* - Create a new .mbs file (similar to the main one), but where all bodies with a changing kinematics
* (i.e. bodies added with the lines 'add_rigid_Ssens' or 'add_rigid_Fsens' in 'user_shapes.cc') are
* assigned to a F sensor (even the ones which should be with a S sensor, due to 'add_rigid_Ssens').
* Like in 'user_shapes.cc', check only the fields 'Position', 'Rotation Matrix' and 'Velocity' for
* the F sensors. Also, remove all the other F sensors in this special .mbs file.
* - Generate the accelred symbolic equations and open the function 'mbs_accelred'
* - Copy these two lines: 'q = s->q;' and 'qd = s->qd;'
* - Copy the sine and cosine equations involved in the requested kinematics computations from 'mbs_accelred'.
* - Copy the whole block 'Sensor Kinematics' from 'mbs_accelred' (stop just before 'SWr1 = user_ExtForces(...);')
* - Add the requested declarations found in 'mbs_accelred_xxx.c' and 'mbs_accelred_xxx.h'
* - For each body where and 'F sensor' was added in the special .mbs file, add this line:
* update_kin_info(PxFi, RxFi, VxFi, OMxFi, kin_info_list, j)
* where 'i' is the index of the F sensor (i.e. in the order of definition in the special .mbs file, starting with '1')
* abd 'j' is the index of the body in the contact model (i.e. the order of calls of 'add_rigid_Ssens'
* and 'add_rigid_Fsens' in 'user_shapes.cc', starting with '0'). To be sure, you can print the value
* 'kin_info_list[j].isens' to know the related index, which would be called in the 'sensor' function.
*/
void user_contact_kinematics(std::vector<KinInfo> &kin_info_list, MainUnionShape *main_union, MBSdataStruct *MBSdata)
{
// Robotran normal computation
main_union->mbs_sensor_compute();
}
/*! \brief update 'kin_info_list'
*
* \param[in] PxF position [m]
* \param[in] RxF rotation matrix [-]
* \param[in] VxF velocity [m/s]
* \param[in] OMxF angular velocity [rad/s]
* \param[out] kin_info_list kinematic information of all moving bodies
* \param[in] kin_info_id ID of the body in the contact model (i.e. order of the rigid shapes added in 'user_shapes.cc')
*/
void update_kin_info(double PxF[4], double RxF[4][4], double VxF[4], double OMxF[4], std::vector<KinInfo> &kin_info_list, int kin_info_id)
{
for(int i=0; i<3; i++)
{
kin_info_list[kin_info_id].P[i] = PxF[1+i]; ///< position [m]
kin_info_list[kin_info_id].V[i] = VxF[1+i]; ///< velocity [m/s]
kin_info_list[kin_info_id].OM[i] = OMxF[1+i]; ///< angular velocity [rad/s]
for(int j=0; j<3; j++)
{
kin_info_list[kin_info_id].R[i][j] = RxF[1+i][1+j]; ///< rotation matrix [-]
}
}
}
}
/*!
* \author Nicolas Van der Noot
* \file user_contact_kinematics.hh
* \brief kinematics of the contact bodies
*/
#ifndef _USER_CONTACT_KINEMATICS_HH_
#define _USER_CONTACT_KINEMATICS_HH_
extern "C" {
#include "MBSsensorStruct.h"
}
#include <vector>
#include "MBSdataStruct.h"
namespace ContactGeom{
// forward declaration
class RigidShape;
class MainUnionShape;
/*! \brief kinematics information
*/
typedef struct KinInfo
{
double P[3]; ///< position [m]
double V[3]; ///< velocity [m/s]
double OM[3]; ///< angular velocity [rad/s]
double R[3][3]; ///< rotation matrix [-]
int isens; ///< Robotran ID in 'sensor'
RigidShape *rigid_shape; ///< RigidShape pointer
MBSsensorStruct sens; ///< Robotran sensor
} KinInfo;
// function prototype
void user_contact_kinematics(std::vector<KinInfo> &kin_info_list, MainUnionShape *main_union, MBSdataStruct *MBSdata);
void update_kin_info(double PxF[4], double RxF[4][4], double VxF[4], double OMxF[4], std::vector<KinInfo> &kin_info_list, int kin_info_id);
}
#endif
#include "user_contact_states.hh"
namespace ContactGeom{
/*! \brief user possible contact states update
*
* \param[in,out] shape current shape to (possibly) update
* \param[in,out] MBSdata Robotran structure
*
* This function is called once during each time step, for each basic shape.
* It can be used for instance to update a FSM (Finite State Machine),
* switching from stiction to sliding.
*
* You can use 'shape->get_module()' to access this shape own module (defined in 'UserShapeModule.hh').
*/
void user_contact_states(BasicShape *shape, MBSdataStruct *MBSdata)
{
}
}
/*!
* \author Nicolas Van der Noot
* \file user_contact_states.hh
* \brief user own states for contact model
*/
#ifndef _USER_CONTACT_STATES_HH_
#define _USER_CONTACT_STATES_HH_
#include "BasicShape.hh"
#include "MBSdataStruct.h"
namespace ContactGeom{
// fucntion prototype
void user_contact_states(BasicShape *shape, MBSdataStruct *MBSdata);
}
#endif
#ifndef STANDALONE
#include <cmath>
#ifdef _CHAR16T // math.h version
#define CHAR16_T // visual version
#endif
#endif
#include "user_shapes.hh"
#include "add_shapes.hh"
#include "RigidShape.hh"
#include "simu_def.h"
namespace ContactGeom{
/// get - for 0, + for 1
inline double plus_minus(int i) { return !i ? -1.0 : 1.0; }
#define DEG_TO_RAD (M_PI / 180.0)
/*! \brief configure the user shapes added in the contact model
*
* \param[in,out] main_union main union of shapes
* \param[in,out] MBSdata Robotran structure
*
* There are three different types of rigid shapes (class RigidShape)
* - fixed: they don't move during the simulation, they apply forces on other bodies,
* but they are not impacted by these forces.
* - related to a S sensor: they move with the body where the S sensor is attached.
* Similary to the fixed rigid shapes, they apply forces on other bodies,
* but they are not impacted by these forces.
* - related to a F sensor: they move with the body where the F sensor is attached.
* They apply forces on other bodies and are impacted by the opposite of
* these forces (action-reaction).
*
* For the shapes related to a S sensor or a F sensor, you must activate the following
* fields in MBSysPad for these sensors:
* - Position
* - Rotation Matrix
* - Velocity
* The last two fields ('Acceleration' and 'Jacobian Matrix') are useless for this contact model.
* So, uncheck them to improve the computational speed.
*
* Each RigidShape belongs to a union shape (UnionShape): a union of one or more shapes.
* A union shape can contain other union shapes (daughters) or rigid shapes. The ancestor
* of all union shapes is a special union shape: main_union (provided as argument).
*
* Except in 'main_union', bodies inside a union shape can only interact with bodies
* inside other unions. On top of that, if applicable (i.e. no plane inside the union),
* wrapping objects are computed around the unions to quickly check if the contact is
* possible. Consequently, add unions when you do not want the objects inside the same
* union to interact and to speed up the computation by indicating unions within which
* objects will probably stay close during the simulation. Consequently, it can be useful
* to make unions of unions to speed up the computation. For instance, if we only consider
* the interactions between a humanoid and its environment, we can place all the bodies
* of this humanoid inside a single union (no interactions computed between the humanoid
* different parts). Then, we can define daughter unions for each arm, leg... because we know
* that the different bodies will stay close to the other bodies in the same daughter union.
*
* To add a union, use this line:
* cur_union = previous_union->add_union();
* where 'cur_union' and 'previous_union' are two UnionShape shapes.
*
* When you want to add a rigid body to the union 'cur_union' (it can also be 'main_union'
* if you do not have any union daughter for this rigid shape), use the line:
* cur_rigid = xxx;
* where xxx can be:
* - add_rigid_fixed_zero(cur_union) : basic rigid shape not moving, with the frame origin
* being the point (0,0,0) (see later)
* - add_rigid_fixed(cur_union, x, y, z) : rigid shape not moving, with the frame origin
* being the point (x,y,z)
* - add_rigid_Ssens(main_union, cur_union, isens) : rigid shape related to the S sensor (reference
* point) with the index 'isens' (starting to 1, coherent with 'sensor')
* - add_rigid_Ssens(main_union, isens) : like 'add_rigid_Ssens(main_union, cur_union, isens)',
* but when the current union 'cur_union' is 'main_union'
* - add_rigid_Fsens(main_union, cur_union, isens) : rigid shape related to the F sensor (reference
* point) with the index 'isens' (starting to 1, add the number of S sensors to get its ID
* in 'sensor')
* - add_rigid_Fsens(main_union, isens) : like 'add_rigid_Fsens(main_union, cur_union, isens)',
* but when the current union 'cur_union' is 'main_union'
* All these fucntions return a pointer to the initialized BasicShape.
*
* Each rigid shape can be made of one or different basic shapes (these shapes can be different
* inside the same rigid shape). Currently, there are three basic shapes: infinite plane, sphere
* and cuboid. The structure of the code is designed to easily allow people to extend this set
* by implementing new basic shapes if needed.
*
* These are the lines you can use to add a basic shape to the rigid shape 'cur_rigid'
* (all arguments are in [m] or [rad]):
*
* - cur_rigid->add_plane(z) : add a plane with the normal in the +z direction, z being its height.
* This function is useful for a flat ground.
* - cur_rigid->add_plane(a,b,c,d) : add a plane with the equation 'ax + by + cz +d = 0'
* - cur_rigid->add_plane(normal_x, normal_y, normal_z, pos_x, pos_y, pos_z) : add a plane whose
* normal is (normal_x, normal_y, normal_z) and its position relative to the reference point
* is (pos_x, pos_y, pos_z). This is expressed in the frame local to the rigid shape.
*
* - cur_rigid->add_sphere(radius, pos_x, pos_y, pos_z) : sphere with a radius 'radius' and a position
* relative to the reference point (pos_x, pos_y, pos_z).
*
* - cur_rigid->add_cuboid(d, w, h, pos_x, pos_y, pos_z, theta_x, theta_y, theta_z) : cuboid with dimensions
* d (depth in 'x'), w (width in 'y') and h (height in 'z'), relative position (pos_x, pos_y, pos_z)
* and relative orientation (theta_x, theta_y, theta_z), all expressed in the frame local to the
* rigid shape.
*
* Shapes inside the same union (except 'main_union') will never interact with each other. By default, all
* the other shapes can have interactions. However, you can change this default behaviour with the function
* 'set_no_contact_default' (its opposite being 'set_contact_default'), as you can see in this example: