Debian Lenny Roboard interaction with Windows Matlab 2010b

Matlab 2010b’s Simulink as a component that allows for insertion of custom code blocks, code generation and allows building real-time applications with Real-Time Workshop (RTW) (http://www.mathworks.com/help/toolbox/rtw/ug/f1133672.html).

This Matlab component is essential for generating code to run on Unix/Linux systems. This tool has lots of customization options and uses, the most relevant of which is the deployment of a Simulink model to an external device and also the External Mode feature that allows for in Simulink to activate and receive logs and stats feedback from the controler running on the remote device.

There are several examples on how to export/compile code for devices supported by Matlab, however we had to look a bit deeper to code generation feature to parameterize it to the right format in order to compile for Roboard Linux. The most important resource in this endeavor was the previous work developed by Dan Bhanderi (http://bhanderi.dk/downloads/) which made a customized soft real-time Linux target, this was updated last for Matlab 2006b hence it required some tunning to be used in 2010b.

Dan Bhanderi adds a System Target to the RTW options menu that opens new options in the interface and when asked to generate code it does so with a makefile for Unix and the lnx_main.c develop by himself to provide the soft real-time solution. This process of creating a new target uses mainly the Target Language Compiler (TLC) and the Template MakeFile (TMF) files that determine the options in the menus and the format of the makefile respectively. If you want to learn more about it I recommend this pdf (http://www.mathworks.com/support/solutions/attachment.html?resid=1-CAFIKO&solution=1-BHU00D) which is taken from Matlab’s website (http://www.mathworks.com/support/solutions/en/data/1-BHU00D/index.html?solution=1-BHU00D), although a bit off-topic I found it very clear and alot easier to read than the main user guides provided.

Dan Bhanderi’s instructions are somewhat outdated, so instead of following this steps exactly we found an utility that upon generating code for a model saves all of it’s code and dependencies to a zip file, the packNGo utility. This utility is very useful to discover the necessary source files to run Simulink engine in the current Matlab instalation be it 2010b or any other. In this way, you always know the smallest amount of code needed to run your code on the device. i.e.: set_param(bdroot, ‘PostCodeGenCommand’, ‘packNGo(buildInfo, {”packType” ”hierarchical” ”fileName” ”BioloidControl”});’); (http://blogs.mathworks.com/seth/2008/11/26/wheres-the-code/)

Since we are not using Dan Bhanderi’s TLC or TMF (just using the generic real-time target) we now need to update the makefile and the main to work in the Unix environment. this process involves updating the makefile with the correct filepaths, included libraries (from code used in possible the s-functions in the model), host and make command, and updating the main file with Bhanderi’s soft real-time code (that uses the POSIX timer).

Warning: When transferring files from other operating systems to Unix be careful to update the newline symbols accordingly. (google dos2unix)

After this process we have the Simulink running on the Roboard, however we whant to see what is happening so we need to activate the External Mode. To do this you access the RTW options under Interface and select External Mode instead of None in the data exchance group. In the mex-file arguments you need to supply the target’s location (so Matlab knows where to get the data) (http://www.mathworks.com/help/techdoc/matlab_external/bp_kqh7.html)

Now if you run the executable in the device with the “-w” parameter it will wait for a connection from Matlab/simulink, upon doing so it will update the scopes data with the values present in your device  (you might have to adjust some settings under the menu Tools->External Mode Control Panel).

EZPhysics communicating with Matlab

The design and development of humanoid control is in it self a research area. Control experts usually use Matlab to easily experiment several approaches and values to achieve the desired model of control.

To allow for a better visual feedback and prototyping, a Matlab s-function was developed to communicate with EZPhysics simulation. This is done through an UDP channel using the HawkNL library to access network features both in Matlab and EZphysics.

Matlab application will Listen to port specified in the function parameters and EZPhysics application will connected to an IP and Port specified in the World Node.

The information sent between the two applications is only 32 bytes client sending and 36 bytes receiving. We send from the client the joints rotational positions and the yaw,pitch and roll of the torso. And Matlab sends the instruction (CW, CCW, HOLD), the torque value (one byte resolution between 0 and MAX_TORQUE) and a third byte of information for future use:

To download this demo visit source forge (the source code available the isn’t updated yet):

https://sourceforge.net/projects/ezphysics/files/EZPhysics%20Matlab%20Module.zip/download

ODE’s servo simulation

In our project, we need to influence the robot’s movements by applying rotational force to robot’s motorized joints.  Applying Torque values to the joints requires calculations that are prone to errors, some of which are dependent on robot’s simulated environment. In addition, the physics engine would take some steps to update the velocity based on the force, which in the case of servos, is irrelevant since they work at supposedly constant speed (not necessarily consistent speed, as seen in previous post by Ricardo) .

However, the design of ODE was made thinking on the simulation of servos (and motors in general). Due to this, beyond the possibility to apply a force to a joint, it is also possible to set the joins parameters with desired speed and maximum applicable torque to achieve that speed. This allows us to create a very simple model that represents a real servo.

As the names suggest, desired speed is a constant that represents the required velocity  either positive or negative. The maximum torque is the absolute maximum force that the servo can apply to achieve the desired speed. For instance, with desired speed zero the engine will resist external forces up to maximum torque effectively simulating a PID servo attempting hold position.

To take advantage of this feature EZPhysics Editor was updated to allow for the settings of this parameters by pressing a key, which allows for the rapid prototyping of the scale of forces needed to move the hinges:

We added a new option in the control node of EZPhysics to allow either the application of a force/torque to a joint or the application of a motor with fixed speed. The flag (ForceOrTorque —  not such a great choice of words) indicates wether that control node applies a torque or applies a simulated motor.

Diving into Control Mechanics

A simple example that show the servos remotely controlled working as expected  (gravity is low):

A Messy Mesh overflows the Mesh Masher (collision detection)

Upon implementing collision detection with GIMPACT (the package used by Bullet)  in ODE, our physics engine, the results were highly unsatisfying. It runs at sensibly the same speed has OPCODE (ODE original collision detection package) and it also crashes with a stack overflow if the complete bioloid is added to the collision detection scene.

When this problem was investigated it turn out that the problem afflicted ODEs sample bunnies also and not only our simulation.

However, upon following a topic found online it was  revealed that the cause of this crash was the meshes inside the meshes. Meaning the bunnies were inside each other and the collision detection algorithm behaves very poorly under these circumstances. The collision detection expects meshes that represent the colliding area of an object, meaning only the outer edges of it. For instance, this simulation has more bunnies and it does not overflow:

Returning to our model, we realize that the SolidWorks exported mesh is very messy. There is excessive detail and many interior triangles (the kind the collision detection algorithm does not expect). In the following image some problems are highlighted but many others are present in the image:

The solution path to this problem is a simple yet a hard one, the editing of all problematic vertexes to achieve a good behavior when inserted into ODE.

EZPhysics – adding mass properties interactively

EZPhysics is a to-be open source project focused on robot simulation using the ODE physics library. Given that ODE may be the best choice for this project’s goals and all the extra features included in EZPhysics it is now our main simulation environment.

This transition has been in progress for a while now and has started to give the initial results:

As a way to dive into the EZPhysics code, I added the possibility of interactively adding mass properties to ODE body entities. Original version was limited to calculating mass and inertial automatically from proxies and their mass density and shape.

By allowing the addition of mass property, it is now possible to export values from SolidWorks into the simulation. However, SolidWorks program exports peculiar aligned inertia matrix and center of mass which has been a source of trouble and odd bugs.. @$!

A Ragdoll in EZphysics (ODE)

The simulation parameters used with the PhysX based simulator have now ported into EZphysics with similar results:

The simulation is slower than with PhysX, however it will probably improve with parameter tuning and/or simpler collision meshs.

Jacobian’s joint limits

When trying to physically simulate the Jacobian robot is necessary to know what are the servos maximum and minimum angle for each joint so that we don’t rely on the collision, easing the simulation (the physics simulator will still emulate collision as if the rotation axis had a mechanical limit).

The Robotis’ Bioloid kit we are using has AX-12 servos. These servos can return the current position of the servo on a given moment. To get the maximum and minimum angle of each joint, we simply adjusted the real servos to the extremes and sampled the position using the Bioloid’s CM5 default firmware and a serial terminal emulator on the PC.
The process of sampling the position is fairly simple, upon connecting the robot with the Boiloid software in the computer, the serial terminal allows interactive chat with the CM5. There is a unique ID to every servo on the bus. To change the servo ID context the command is “cid id”. Once in the servos’s scope inserting “read 36 2” will return two bytes: the first for the Low value and the second for the High value. This represents a range from 0 to 1023 which is mapped to 0º to 300º (See below). The values increase clockwise if moving the part and not the rotation cylinder, vice-versa if you are thinking in the perspective of the cylinder.

The sampled data for the Jacobian:

 

Right Leg:

(Joint 1) CID 6    ->  166-821 (3*(768)+53)            48º633-240º527 (range 655 191º895 ~ 2x96º)
(Joint 2) CID 4    -> 209-866 (3*+98)                 61º230-256º347 (range 657 192º480 ~ 2x96º)
(Joint 3) CID 15  -> 416-875 (1*(256)+160)-(3*+107) 121º875-256º348 (range 459 134º473 ~ 2x67º)
(Joint 4) CID 17  -> 246-769 (3*+1)                  72º070-225º293 (range 523 153º223 ~ 2x76º5)
(Joint 5) CID 40  -> 000-613 (2*(512)+101)            0º000-179º590

Left Leg:

(Joint 6) CID 41     -> 753-1023(2*+241)-(3*+Null+65)  120º605-300º000
(Joint 7) CID 19    -> 270-764 (1*+14)-(2*+252)        79º102-223º828 (range 494 144º727 ~ 2x72º)
(Joint 8) CID 11    -> 157-601 (2*+89)                 45º996-176º074 (range 444 130º078 ~ 2x65º)
(Joint 9) CID 18    -> 174-797                         51º026-233º496 (range 623 182º520 ~ 2x91º)
(Joint 10) CID 14 -> 203-858                         59º472-251º367 (range 655 191º895 ~ 2x96º)

Model Lying down values:

Right Leg:

(1)   6 - (1*+244)         ~ 150º
(2)   4 - (1*+246)         ~ 150º
(3)  15 - (1*+246)         ~ 150º
(4)  17 - (2*+001)         ~ 150º
(5)  40 - (2*+001)         ~ 150º

Left Leg:

(6)  41 - (2*+001)         ~ 150º
(7)  19 - (2*+010)         ~ 150º
(8)  11 - (2*+004) 151º172 ~ 150º
(9)  18 - (1*+254)         ~ 150º
(10) 14 - (1*+252)         ~ 150º

As you can see, the precision of this mechanism may not be great, however for the current purpose of applying some limits in the simulation this values are good enough. Also keep in mind that some positions (in our case the lying down position with 150º on all engines) should be easy to model in 3D (all aligned, parallel or perpendicular), because it will serve as our reference orientation in the simulation.

As previously mentioned, the servo only returns a valid position between 0º and 300º. This can be an issue, for instance, in the 40 and 41 servos we sampled. Although previous planing ensures that this servos can reach this positions it will not be necessary or desired.

This sampling lead to this approximate value:

To apply this limits to the PhysX representation of the model we use the joint object properties. In our case, we only need the NxD6Joint’s twist limit property, this property takes a high and a low value that represent the limit angles of twist. However, these values need to respect the following relation:

Hence, determining where the 0º angle is going to be is important. To place the 0º degree angle at the right place it is necessary to use the NxJoint local normal property. Modeling all the parts in the same axis will help to determine this values, please refer to the PhysX documentation on how these values should be set for they will be different depending on the way the models are built.

PhysX servo’s limit implementation:

//It is predefined that all actors have the same axis
joint.localAxis[0] = joint.localAxis[1] = localAxis;
joint.localNormal[0] = joint.localNormal[1] = normalAxis;

joint.twistMotion = NX_D6JOINT_MOTION_LIMITED;
joint.twistLimit.high.value = highTwistLimit;
joint.twistLimit.low.value = lowTwistLimit;

Version 0.1 of the simulator

Upon reaching this landmark in the development of the Jacobian simulator, we provide all the source code and models used to make the videos posted:

Jacobian Simulator v0.1(zip)

Please refer to the ReadMe.txt inside the folders for instructions on how to compile and use the application. An important remark: it is necessary to have the PhysX drivers installed to run the simulation (even without a PhysX enabled graphics card).

The following scheme is the data flow scheme of the work developed:

It falls!

The jacobian can now collide with the surrounding environment:

This simulation demonstrates a ragdollish fall of the Jacobian model interacting with the floor.

The light blue leg (the left one) represents the convex approximation of the model real shape, the real shape is represented by the right leg (with a darker blue). It is visible the similarity between these two shapes.

The PhysX debug view is toggled and the orange lines represent the convex meshes used.

Some cubes are thrown into the scene interacting with the robot and floor and indicating that the collision with the environment works correctly.

It moves!

In the videos below you can see some preliminary tests on applying sinusoidally sequenced torque to the parts that compose the Jacobian Model. There is a “bug” in the way that the torque is applied. Instead of applying it to the body parts in relation to the servo rotation axis, the torques are applied in relation to an axis that is at the center of mass of the body parts. Obviously this is not really the way things work with a multi-servo robotic construct in the real world. The torque in the real world is applied by the servo along the rotation axis.

The good news is that convex triangle mesh based collision seems to work.

Jacobian robot simulation with a sinusoidal signal applied to the legs:

A froggy Jacobian?

Mesh Collision in PhysX

Apart from the simple shapes as a cube or capsule, PhysX allows for Convex Meshs and Triangle Meshs to represent more complex collision objects. Unfortunately, it is usually not possible to use the 3D triangle mesh used for display purpose as-it-is for collision detection. Also it is very important to keep in mind that mesh collision is a computationally expensive operation and should be avoided when possible. Another important fact is that Triangle Mesh to Triangle Mesh collision is not supported by PhysX.

A 3D triangle mesh is loaded into OpenGL or DirctX as a list vertexes in the form of XYZ coordinate triplets (v0,v1,v2), triangles as a list of indexes referring to the previous defined vertexes, may have vectors representing the triangles surface normal (indicating the side of the triangle where the texture would be mapped to facing “outwards”), texture pointers (or materials) associated with the triangles and eventually animation information. A small piece of a model in obj format:

v 0.014000 -0.039000 0.012000 //vertex position

v 0.014000 0.039000 0.012000

usemtl MediumGrey //Material information

f 196 209 235 //triangle (or face) information

f 209 215 235

In order to transform a 3D triangle mesh into a collision object PhysX provides an application framework. This process is called cooking by PhysX documentation. The mesh provided as input cooking rarely should be the same than the one being shown on screen, because of the excess of detail and consequent overhead in the physics calculation. A simplified model should be used, this model may, for instance, be convex (which provides significant less overhead in the simulation)  and should consider other minor aspects, such as, reduction of vertex duplication.

When the mesh is ready it needs to be “cooked” by PhysX, this process can be long but is a one-time-only operation. The output is file in binary format that is optimized for collision detection and can be easily loaded in the shape it represents at run-time. The following code uses the PhysX SDK’s 3DS model wrapper that contains the 3d rendering mesh (other file types and wrappers will have different parameters) and “cooks” it into a collision mesh.

Code:

Model_3DS* m = (Model_3DS*)(((ShapeUserData*)robot->getShapes()[0]->userData)->model);

bool success = gCooking->NxCookTriangleMesh(Desc, UserStream(“C:/cooked.bin”, false));

NxCookingInterface *gCooking = NxGetCookingLib(NX_PHYSICS_SDK_VERSION);

gCooking->NxInitCooking();

//Build physical model

NxTriangleMeshDesc Desc;

Desc.numVertices = m->Objects[0].numVerts;

Desc.numTriangles = m->Objects[0].numFaces/3;

Desc.pointStrideBytes = sizeof(Vertex);

Desc.triangleStrideBytes = 3*sizeof(unsigned short);

Desc.points = m->Objects[0].Vertexes;

Desc.triangles = m->Objects[0].Faces;

Desc.flags = NX_MF_16_BIT_INDICES;

bool valid = Desc.isValid();

bool success = gCooking->NxCookTriangleMesh(Desc, UserStream(“C:/cooked.bin”, false));

In my experience I had another curious difficulty, nothing returned errors but I could not see the mesh. Turns out it was the scale. The scale of a cooked collision mesh (convex or triangle) cannot be changed, so be sure to define the scales used before hand.

Upon doing so it is only necessary to create a TriangleMeshDescriptor load the mesh in, associate it with a Shape, the Shape with the Actor and collision is ready!

The tiny TriangleMesh:

The final Triangle Mesh in red on top of the rendered mesh in blue:

Managing simulated Jacobian’s data

Upon investigation of the select physics engine (PhysX), a list of the Jacobian’s properties as been assembled. Each part must have the following PhysX simulation data:

• part position & orientation
• mass
• center of mass offset position & orientation
• mass space inertia tensor (diagonalized) & rotation
• motor’s limits, maximum force and position & orientation
• part’s material, with friction and bounciness (for feet only)
• part’s shape, only for collision purposes

The parts also have a triangle mesh and associated textures kept together with the physical data to allow for visual feedback. The Collada 1.4.1 file can hold all this information and it is our intention that once all the data has been verified for it to be saved  in this standard allowing for easy editing of the parameters once the application is complete.

With the orientation and shape information missing, our 3D model currently can be represented by rectangles equivalent to the inertial moment of the real robot:

The small XYZ are indicating the center of mass of the part.

The Simulation Pipeline

During the past two weeks we have started developing the real-time 3D physics simulation of the robot and its control.

The first step in this endeavor was to find the best tools to simulate Jacobian in a virtual environment.

Physics simulation involves: