Commit 1faf70ad authored by Timothee Habra's avatar Timothee Habra

add options to specify primitive geometry for simbody contact

parent c9849a44
......@@ -24,8 +24,17 @@ typedef struct ContactPropertiesStruct
double ScaleFactor; //how to scale a mesh
double Rotation[3]; //how to rotate a mesh
double Transform[3]; //how to shift a mesh
char FileName[LENGTHOFFILENAME];
int Geometry; // 0 means OneHalfSpace; 1 means mesh
// specific to mesh contact
char FileName[LENGTHOFFILENAME];
// specific to sphere primitive contact
double radius; // sphere radius
// specific to box primitive contact
double box_dim[3]; // half length of the box
} ContactPropertiesStruct;
typedef struct SimbodyBodiesStruct
......
......@@ -317,7 +317,7 @@ try
printf("\n k = %lf \n",CurBodyContProp->k);
Body.addContactSurface(Transform(Vec3(0,0,0)),
Body.addContactSurface(Transform(Vec3(CurBodyContProp->Transform)),
ContactSurface(BodyTrM,
ContactMaterial(CurBodyContProp->k,CurBodyContProp->c,CurBodyContProp->us,CurBodyContProp->ud,CurBodyContProp->uv),
CurBodyContProp->thickness)
......@@ -325,33 +325,33 @@ try
// it is only for visualization. Doesn't work under Linux
#ifdef VIZ
DecorativeMesh showBox = DecorativeMesh(BodyMesh);
Body.addDecoration(Transform(), showBox.setColor(Red).setOpacity(1).setRepresentation(SimTK::DecorativeGeometry::Representation(0)));
Body.addDecoration(Transform(Vec3(CurBodyContProp->Transform)), showBox.setColor(Red).setOpacity(1).setRepresentation(SimTK::DecorativeGeometry::Representation(0)));
#endif // VIZ
printf(" succeed! \n\n");
}
else if (CurBodyContProp->Geometry == 2) // box
{
Body.addContactSurface(Transform(Vec3(0,0,0.2)),
ContactSurface(ContactGeometry::Brick(Vec3(0.075,0.075,0.075)),
Body.addContactSurface(Transform(Vec3(CurBodyContProp->Transform)),
ContactSurface(ContactGeometry::Brick(Vec3(CurBodyContProp->box_dim)),
ContactMaterial(CurBodyContProp->k,CurBodyContProp->c,CurBodyContProp->us,CurBodyContProp->ud,CurBodyContProp->uv),
CurBodyContProp->thickness)
);
#ifdef VIZ
Body.addDecoration( Transform(),
DecorativeBrick(Vec3(0.075, 0.075, 0.075)).setColor(Red).setOpacity(1));
Body.addDecoration(Transform(Vec3(CurBodyContProp->Transform)),
DecorativeBrick(Vec3(CurBodyContProp->box_dim)).setColor(Red).setOpacity(1));
#endif
}
else if (CurBodyContProp->Geometry == 3) // sphere
{
Body.addContactSurface(Transform(Vec3(0,0,0)),
ContactSurface(ContactGeometry::Sphere(0.02),
Body.addContactSurface(Transform(Vec3(CurBodyContProp->Transform)),
ContactSurface(ContactGeometry::Sphere(CurBodyContProp->radius),
ContactMaterial(CurBodyContProp->k,CurBodyContProp->c,CurBodyContProp->us,CurBodyContProp->ud,CurBodyContProp->uv),
CurBodyContProp->thickness)
);
#ifdef VIZ
Body.addDecoration(Transform(),
DecorativeSphere(0.02).setColor(Orange).setOpacity(1));
Body.addDecoration(Transform(Vec3(CurBodyContProp->Transform)),
DecorativeSphere(CurBodyContProp->radius).setColor(Orange).setOpacity(1));
#endif
}
......
......@@ -140,7 +140,11 @@ int fill_bodies_contact_properties(ContactPropertiesStruct* BodyContProp, int Nu
CurBodyContProp->Rotation[1] = 0;
CurBodyContProp->Rotation[2] = 0;
CurBodyContProp->Geometry = 1; // // 1 means a mesh, 2 a box, 3 a sphere
CurBodyContProp->Geometry = 2; // // 1 means a mesh, 2 a box, 3 a sphere
CurBodyContProp->box_dim[0] = 0.075;
CurBodyContProp->box_dim[1] = 0.075;
CurBodyContProp->box_dim[2] = 0.075;
sprintf(CurBodyContProp->FileName, "cube.obj"); // file should be in the folder src/project/simulation_files/Simbody/mesh_obj
......@@ -169,6 +173,9 @@ int fill_bodies_contact_properties(ContactPropertiesStruct* BodyContProp, int Nu
CurBodyContProp->Geometry = 3; // 1 means a mesh, 2 a box, 3 a sphere
CurBodyContProp->radius = 0.02;
sprintf(CurBodyContProp->FileName, "r_wrist.obj"); // file should be in the folder src/project/simulation_files/Simbody/mesh_obj
......@@ -196,6 +203,7 @@ int fill_bodies_contact_properties(ContactPropertiesStruct* BodyContProp, int Nu
CurBodyContProp->Geometry = 3; // 1 means a mesh, 2 a box, 3 a sphere
CurBodyContProp->radius = 0.02;
sprintf(CurBodyContProp->FileName, "l_wrist.obj"); // file should be in the folder src/project/simulation_files/Simbody/mesh_obj
//*/
......
......@@ -27,7 +27,7 @@ Installation of Simbody
-------------------------------
Simbody is an open-source, object-oriented C++ API, and its source codes can be download from [here](https://simtk.org/project/xml/downloads.xml?group_id=47).
We used Simbody 3.3.1 Release on Windows and Linux OS. Note that you should build 64-bit version of this library to be compatible with the whole project.
Note that you should build 64-bit version of this library to be compatible with the whole project.
Instructions of building Simbody can be found on the Simbody official site:
* [Mac and Linux](https://simtk.org/docman/view.php/47/1583/HowToBuildSimbodyFromSource_MacLinux.pdf)
......@@ -41,58 +41,27 @@ Definition of contact properties
-----------------------------------
### To define S- and F-sensors ###
Add S- and F-sensors to the bodies that you want to have contact with ground or each other.
In your model, add S- and F-sensors to the bodies that you want to be in contact with ground or with other contact bodies (e.g. you can use Robotran MBSysPad to add these sensors).
Sensors should be placed in the origin of body coordinate frame.
To get IDs of the sensors run a script in Matlab getindices.m (TODO! in template there should be this script):
`prjname = 'OffRoadRobot'; % project name`
`[mbs_data, mbs_info] = mbs_load(prjname,'default');`
`id_S_FR_Sensor = mbs_get_S_sensor_id(mbs_info,'FR_Sensor'); % do not forget to change name to that you specified in MBSysPad`
`fprintf('// S sensors\n');`
`fprintf('#define S_FR_Sensor %d\n', id_S_FR_Sensor);`
`%% F-Sensors`
`id_F_FR_Sensor = mbs_get_F_sensor_id(mbs_info,'FR_Leg_Force');`
`fprintf('// F sensors\n');`
`fprintf('#define F_FR_Sensor %d\n', id_F_FR_Sensor);`
### Fill the contact properties ###
First, define the header file "SimbodyBodiesDefinitions.h":
* Define a number of contact bodies (except the ground that does not move during simulation. If in your project you need a moving ground, then you should add it like others bodies)
`#define NB_CONTACT_BODIES 4`
* Add IDs for sensors - just copy the output that appeared in Matlab workspace in step 1 and put it here
* Define a number of contact bodies (except the ground that does not move during simulation. If in your project you need a moving ground, then you should add it like others bodies).
`// S sensors`
`#define S_FR_Sensor 1`
`...`
`// F sensors`
`#define F_FR_Sensor 1`
E.g. `#define NB_CONTACT_BODIES 4`
* Specify arrays for sensors IDs. S- and F- sensors of different bodies should go in the same order in these arrays.
So the algorithm considers that first members of these arrays are attached to the first body in Simbody world,
second members to the second body and so on.
So the algorithm considers that first members of these arrays are attached to the first body in Simbody world, second members to the second body and so on.
`#define S_SENSORS_ARRAY {S_FR_Sensor, S_FL_Sensor, S_RL_Sensor,S_RR_Sensor}`
```
`#define F_SENSORS_ARRAY {F_FR_Sensor, F_FL_Sensor, F_RL_Sensor,F_RR_Sensor}`
#define S_SENSORS_ARRAY {1, 5, 4, 9}
#define F_SENSORS_ARRAY {4, 7, 6, 5}
```
Second, specify coefficients of contact model and meshes in the file "ContactPropertiesDefinitions.cpp".
* Second, specify coefficients of contact model and meshes in the file "ContactPropertiesDefinitions.cpp".
There you have two functions to fill. The first one 'fill_ground_contact_properties' defines properties of ground (TODO! to add a flag to specify if ground exists in the model),
the second one 'fill_bodies_contact_properties' is used for bodies (you should fill the properties in the order you used for sensors in the previous file).
......@@ -100,45 +69,69 @@ You need to fill the following fields:
1. Mechanical parameters of the contact (link to the description of this parameters is in the "Comments" paragraph below):
`BodyContProp->ud = 0.9; // dynamic dry friction coefficient`
`BodyContProp->us = 1.1; // static; it is required ud < us`
`BodyContProp->uv = 0; // viscous (force/velocity)`
```
`BodyContProp->c = 1e3; // dissipation (1/v)`
BodyContProp->ud = 0.9; // dynamic dry friction coefficient
BodyContProp->us = 1.1; // static; it is required ud < us
BodyContProp->uv = 0; // viscous (force/velocity)
BodyContProp->c = 1e3; // dissipation (1/v)
BodyContProp->k = 5e3; // stiffness (pascals)
BodyContProp->thickness = 0.01;
```
2. Type of geometry.
To run faster, some primitive geometries can be used.
* For ground '0' here means OneHalfSpace z+ (e.g. flat horizontal ground);
* '1' means a mesh defined by a file (the more general case but also the slowest).
* '2' a box
* '3' a sphere
`BodyContProp->k = 5e3; // stiffness (pascals)`
Note : the contact between all geometries might not be available (refer to Simbody documentation). E.g. at the present time box contact (simbody bricks) is only available for half space).
`BodyContProp->thickness = 0.01;`
2. Type of geometry. For ground '0' here means OneHalfSpace z+; '1' means a mesh defined by a file.
(TODO! may be here -1 will be to specify if there is no ground;
for bodies we can specify different primitives such as sphere, box, etc.)
`BodyContProp->Geometry = 1; // 0 means OneHalfSpace z+; 1 means a mesh`
```
BodyContProp->Geometry = 1; // 0 means OneHalfSpace z+; 1 means a mesh
```
3. Transformation of geometry - you should use the same numbers as in MBSysPad for vrml meshes.
These fields are only applicable for mesh ground only (TODO! to add possibility to have inclined or shifted ground)
`BodyContProp->ScaleFactor = 0.5; // scaling factor. Applicable only for mesh`
```
`BodyContProp->Transform[0] = -2.5; // translation in X, (TODO! to change name of the field from transform to translate? )`
`BodyContProp->Transform[1] = -2; // ... Y,`
BodyContProp->ScaleFactor = 0.5; // scaling factor. Applicable only for mesh
BodyContProp->Transform[0] = -2.5; // translation in X, (TODO! to change name of the field from transform to translate? )
BodyContProp->Transform[1] = -2; // ... Y,
BodyContProp->Transform[2] = -0.05; // and Z directions
BodyContProp->Rotation[0] = 90; // [degrees] Rotation about X,
BodyContProp->Rotation[1] = 0; // Y,
BodyContProp->Rotation[2] = 0; // Z axis.
```
4. Geometry details
* For a mesh specify the mesh file
`BodyContProp->Transform[2] = -0.05; // and Z directions`
```
`BodyContProp->Rotation[0] = 90; // [degrees] Rotation about X,`
sprintf(BodyContProp->FileName, "ground_mine.obj"); // file should be in the folder \Standalone\src\project\simbody` (TODO! Where?)
```
* For a box, specify the half-length
`BodyContProp->Rotation[1] = 0; // Y,`
```
`BodyContProp->Rotation[2] = 0; // Z axis.`
4. Name of a file describing a mesh
CurBodyContProp->box_dim[0] = 0.075;
CurBodyContProp->box_dim[1] = 0.075;
CurBodyContProp->box_dim[2] = 0.075;
```
* For a sphere, specify the radius
`sprintf(BodyContProp->FileName, "ground_mine.obj"); // file should be in the folder \Standalone\src\project\simbody` (TODO! Where?)
```
CurBodyContProp->radius = 0.02;
```
Switching on flags in CMake
---------------------------------------
......
......@@ -8,7 +8,7 @@
//#include "simu_def.h"
//#define VIZ
#define VIZ
// path to the .obj mesh files
#define BODIES_OBJ_PATH PROJECT_ABS_PATH,"/src/project/simulation_files/Simbody/mesh_obj"
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment