Capturing software-skinned mesh data

EZphysics highlights meshes and proxies when you select them in the menu-tree.

The way highlighting works is that the edges of the triangle mesh are colored with alternating colors so the mesh is displayed and overlayed by the alternating colored lines. However it seems that there is a problem with accessing the skeletal mesh triangle data *after* it has been skinned (the process of modifying a “skeleton” where each bone influences a group of points on the mesh hence skinned mesh refers to displaying the altered mesh after its bones have changed position). We tried to force skinnnig to occur in software rather then GPU and it seems to be doing that but still capturing the elusive post-skinned mesh is yet to be achieved as depicted in the image below where the girl’s mesh has been transformed using skeletal animation technique but the highlighted triangle contour is in its original pose.

Communication protocol between PC and CM5

The communication is based on the need of controlling the servos position, speed and torque according to the  mathematical algorithms, in this case we need servos position, speed and load as inputs to the algorithms and the outputs will be new servos positions and torque.

To do this communication between the servos and the PC efficient and fast, is used the CM5 as intermediate.

The CM5 has a firmware that keeps running in loop refreshing the servos data, checking PC new incoming data to servos and retrieving the new servo data to be sent to PC, each cycle.

To do this, CM5 have three stages since it boots up.

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:

Simulink model of Jacobian

The Simulink model of Jacobian has a close loop for each joint with a PID controller to keep all joints in a predefined position, another close loop to simulate the contact with the ground and a block to estimate the global center of mass of Jacobian.
The simulink model was built with SimMechanics blocks where each body block has the values of mass, center of mass and inertia (from here) and joints coordinates acquired from 3D model.
In this model is possible to simulate movements of joints but, at a moment, the knee and hip rotations is equals to the both legs.

You can download the report and matlab/simulink files.

The next steps are the control of equilibrium of Jacobian. First we will control the equilibrium with two legs on the ground and later with only one.

To finalize the equilibrium control of Jacobian, we will do it when the humanoid stand down and up, bending the knees.

Mass proprieties of jacobian – Dynamixels in center position

In this study, we repeted the  process to acquire the mass proprieties of Jacobian with the all dynamixels in center value. This pose is better than the another one (in approximate equilibrium point) because we has better information about the rotation angle of joints.

We use the Mass Properties tools of Solid Works to get the proprieties of each individual body. In the first stage we performed a full body analyze, and later we suppress 10 assemblies (assembly of parts for each individual body) for each analysis to get proprieties of individual body parts.

The properties of this study were defined with all dynamixels in center value and should be available here.

These are the values that we used in the MatLab/Simulink model

Mass Properties of Jacobian (in equilibrium point)

The Kangaroo (a.k.a Jakobian1) has a multi-body structure with 11 bodies and 10 joints.

In this study, we used the Mass Properties tools of Solid Works to get the proprieties of each individual body. In the first stage we performed a full body analyze, and later we suppress 10 assemblies (assembly of parts for each individual body) for each analysis to get proprieties of individual body parts.

The properties of this study were defined with a robot pose approximated at the equilibrium point and should be available here.

We will use this values to build a model in MatLab/Simulink.

Jacobian model in SimMechanics (MatLab/Simulink)

We did a first approach to build the jacobian dynamic model in MatLab 2009b / Simulink using the blocks of SimMechanics Toolbox.

In this first approach, we used he properties of the structure and body parts with a robot pose approximated at the equilibrium point.

The SimMechanics blocks needs some parameters (center of mass and inertia matrix of each body part and joint coordinates) that are obtained in report “Mass Properties of Jacobian – equilibrium point”

To view the simulation, download the model, start the Matlab 2009b (the model could not open in prior versions), open the Simulink model and click in “Start” and view the structure in the machine for model.

3D Model of ‘Jacobian1’ the Kangaroo Robot

Using the 3D models of components from the Bioloid robot kit available from http://humanoids.dem.ist.utl.pt, we built in SolidWorks the 3D model of our Kangaroo Robot (download SolidWorks Model).

The objective is to calculate the mechanical proprieties (inertial moment, weight, center-of-mass) of each part of robot’s body (torso, hip, upper leg, etc…) and also create a WRL model that can be animated by MatLab/Simulink.

The Kangaroo (a.k.a Jakobian1) has 11 different sub-assemblies/parts (torso and two for each hip, upper and lower leg and knee for each leg). Mechanical properties need to be calculated for each in order to model the structure and be able to control its motion.

By enabling axis constraints, we can even mold the robot into some inspirational poses.

Dynamixel bus communication from Matlab

Using a board with FTDI chip, and the library from Robotis, the DLL is linked to Matlab and so functions can be called from within m-functions to send-receive packets from the servo bus.

function [PresentPos]=bioloid(id,GoalPos)
% Send and receive values from dynamixels
% [PRESENTPOS]=BIOLOID(ID,GOALPOS)
%       ID - Vector with all dynamixels ID
%       GOALPOS - Vector with all goal dynamixels positions
%       PRESENTPOS - Vector with all dynamixels positions

% Fernando Carreira
% IST-Humanoids lab, Lisbon
% 09-02-2010

n=length(id);
for pos=1:n
    calllib('dynamixel','dxl_write_word',id(pos),30,GoalPos(pos));      %write pos
    PresentPos(pos) = calllib('dynamixel','dxl_read_word',id(pos),36);  %read pos
end

Using the CM5 as translator/compressor

The problem with USB’s inherent latency due to the USB bus controller requiring a cycle of 1ms by definition (maybe USB3 will change this in the future), can be addressed by having long strings sent from the Robotis Dynamixel bus to the PC instead of probing the bus for every servo continuously.

So we will try to use the CM5 as a middle-man. The CM5 is the Bioloid controller board. It will peek/poke the servos continuously and send compressed strings to the PC over USB/FTDI at high speed. Theoretically we should be able to reach 100+ cycles/sec of reading all the servo positions and sending out target positions and torque commands from the PC assuming 1mbps emulated serial port speed.

PC -> Servo Communications issues

Robotis makes unique servo actuators specifically designed for building articulated robots. Their actuators range from AX12 ($40 12kg-cm plastic gear) to EX-106 ($500 120kg-cm titanium gear and magnetic encoder..). They all share one thing in common – the Dynamixel bus protocol.

In order to effectively create closed loop control between the PC and the robot, there are many timing considerations.

We debated quite a while about how to best create a situation whereby the PC is able to receive sensor data and send control data from/to the servos at maximum cycles/sec.

The following is stitched up (see hugin.sourceforge.net) picture of the brainstorm board taken as 4 pictures by an iPhone

Long Legs

The reason why hobby humanoids are so unstable and need those big feet is that they are short and their center of mass is at around 20cm height. meaning that upon any perturbation, they will very quickly loose balance and reach an angle where a disastrous full fall can not be prevented.  T=0.897 sec/cycle means that controlling the instability requires reaction at an order of T/4 = 0.22 Sec. Thats about the time it takes the servo to run through 45Deg. If that servo is on the hip of the humanoid, moving the whole leg whose length is about 15cm it can spread that leg about 10cm on the ground. Seems like enough to keep balance but I prefer to do our experiments in a less stressful predisposition.

If the CM is brought up to 35cm, T/4 = 0.297 Sec. Hence more time to react to instability. The downside is that when building this structure from Bioloid plastic parts, there’s now an additional flexibility and springiness to everything.

The reason for double jointed knees is so that the speed of folding and extending those knees is about double the speed of moving the whole leg from the torso.

Hello world!

ActuatedCharacter project proposal was submitted in end of 2007 to Eureka / Eurostars for funding. It was elected to be funded in April 2008. The money showed up in the bank in January 2010.

Our ambitious objective is to create cheap legged bipedal robots that can run and jump.

This is a cooperation between IST (Instituto Superiore Tecnico – University based in Lisbon/Portugal, RoboSavvy (hobby education robotics company based in UK), NoDNA (hobby education robotics company in Germany) and Move-Interactive (Video Game development company based in Portugal).