Commit 31ab6a6a authored by Bruno Laurencich's avatar Bruno Laurencich

merge develop, Release 0.1.1b

parents bd587cb0 6d6a62b4
......@@ -2,8 +2,11 @@
#################
## Chordata specific
#################
a_calibrated_kc.xml
calib/*.csv
calib/octave-workspace
.clang_complete
confs
.sconsign.dblite
*.sublime-*
NOTES
......
<!-- Chordata.xml -->
<Chordata version="0.1.0">
<Configuration>
<KC_revision> 2 </KC_revision>
<Communication>
<Adapter>
/dev/i2c-1
</Adapter>
<Ip>
0.0.00.0
192.168.1.30
</Ip>
<Port>
6569
6565
</Port>
<Log>
stdout file
</Log>
stdout,file
</Log>
<Transmit>
osc
</Transmit>
<Send_rate>
50
</Send_rate>
<Verbosity>
0
</Verbosity>
</Communication>
<Osc>
<Base>
/Papa
/Chordata
</Base>
</Osc>
<Fusion>
<Beta_start>
1.0
</Beta_start>
<Beta_final>
0.2
</Beta_final>
<Time>
5000
</Time>
</Fusion>
</Configuration>
<Armature>
<Mux Name="MAIN" id="0">0x72<Branch Name="AnteBrazo" id="1">CH_4<K_Ceptor Name="AnteBrazoSensor" id="2">
0x00
<calibration type="offset">
351.06 2674.02 -2772.45
</calibration>
<calibration type="matrix">
1.076917 0.072891 -0.013270
0.072891 0.960695 -0.023474
-0.013270 -0.023474 0.972254
</calibration>
</K_Ceptor>
</Branch>
<Branch Name="Brazo" id="3">CH_3<K_Ceptor Name="BrazoSensor" id="4">
0x00
<calibration type="offset">
236.99 -552.77 683.68
</calibration>
<calibration type="matrix">
0.978881 -0.013358 -0.021316
-0.013358 0.990111 -0.020486
-0.021316 -0.020486 1.032867
</calibration>
</K_Ceptor>
</Branch>
<Branch Name="Mano" id="5">CH_2<K_Ceptor Name="ManoSensor" id="5">
0x00
<calibration type="offset">
-904.09 873.90 1275.87
</calibration>
<calibration type="matrix">
9.3938e-01 -3.6198e-02 -2.6104e-02
-3.6198e-02 1.0290e+00 -6.5024e-04
-2.6104e-02 -6.5024e-04 1.0366e+00
</calibration>
<Mux Name="main" id="0">
0x73
<Branch Name="left" id="1">
CH_2
<K_Ceptor Name="Unico" id="2">
0
</K_Ceptor>
</Branch>
</Mux>
</Armature>
</Chordata>
\ No newline at end of file
</Chordata>
......@@ -48,6 +48,12 @@
</xs:annotation>
</xs:element>
<xs:simpleType name="versionType">
<xs:restriction base="xs:normalizedString">
<xs:whiteSpace value='collapse'/>
<xs:pattern value="[0-9]{1,3}\.[0-9]{1,3}\.[0-9]{1,3}" />
</xs:restriction>
</xs:simpleType>
<xs:complexType name="root_type">
<xs:all>
......@@ -56,21 +62,58 @@
<xs:element type="ArmatureType" name="Armature"
minOccurs="1" maxOccurs="1"/>
</xs:all>
<xs:attribute type="xs:float" name="version"/>
<xs:attribute type="versionType" name="version"/>
</xs:complexType>
<!-- CONFIGURATION -->
<xs:simpleType name="CollapsedFloatType">
<xs:restriction base="xs:float">
<xs:whiteSpace value='collapse'/>
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="CollapsedIntType">
<xs:restriction base="xs:integer">
<xs:whiteSpace value='collapse'/>
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="revisionType">
<xs:restriction base="CollapsedIntType">
<xs:minInclusive value="1"/>
<xs:maxInclusive value="2"/>
</xs:restriction>
</xs:simpleType>
<xs:complexType name="ConfigurationType">
<xs:all>
<xs:element type="revisionType" name="KC_revision"
minOccurs="0" maxOccurs="1"/>
<xs:element type="CommunicationType" name="Communication"
minOccurs="0" maxOccurs="1"/>
<xs:element type="OscType" name="Osc"
minOccurs="0" maxOccurs="1"/>
<xs:element type="FusionType" name="Fusion"
minOccurs="0" maxOccurs="1"/>
</xs:all>
</xs:complexType>
<xs:complexType name="FusionType">
<xs:all>
<xs:element type="CollapsedFloatType" name="Beta_final"
minOccurs="0" maxOccurs="1"/>
<xs:element type="CollapsedFloatType" name="Beta_start"
minOccurs="0" maxOccurs="1"/>
<xs:element type="CollapsedIntType" name="Time"
minOccurs="0" maxOccurs="1"/>
</xs:all>
</xs:complexType>
<xs:complexType name="OscType">
<xs:all>
......@@ -118,6 +161,9 @@
<xs:element type="verbosityType" name="Verbosity"
minOccurs="0" maxOccurs="1"/>
<xs:element type="CollapsedFloatType" name="Send_rate"
minOccurs="0" maxOccurs="1"/>
</xs:all>
</xs:complexType>
......@@ -168,7 +214,7 @@
<xs:complexType name="ArmatureType">
<xs:choice>
<xs:element type="MuxType" name="Mux"
minOccurs="0" maxOccurs="1"/>
minOccurs="0" maxOccurs="4"/>
<xs:element type="K_CeptorType" name="K_Ceptor"
minOccurs="0" maxOccurs="1"/>
......@@ -187,6 +233,7 @@
<xs:extension base="calibrationContentType">
<xs:attribute type="calibTypeAttr" name="type" use="required"/>
<xs:attribute type="calibTargetAttr" name="target"/>
</xs:extension>
</xs:simpleContent>
......@@ -199,10 +246,18 @@
</xs:restriction>
</xs:simpleType>
<xs:simpleType name="calibTargetAttr">
<xs:restriction base="xs:string">
<xs:enumeration value="acel" />
<xs:enumeration value="gyro" />
<xs:enumeration value="mag" />
</xs:restriction>
</xs:simpleType>
<xs:complexType name="K_CeptorType" mixed="true">
<xs:sequence>
<xs:element type="calibrationType" name="calibration"
maxOccurs="2" minOccurs="0"/>
maxOccurs="4" minOccurs="0"/>
<xs:element type="K_CeptorType" name="K_Ceptor"
minOccurs="0" maxOccurs="1"/>
......
<!-- Chordata.xml -->
<Chordata version="1.0">
<Armature>
<Mux Name="MAIN" id="0">0x72<Branch Name="AnteBrazo" id="1">CH_4<K_Ceptor Name="AnteBrazoSensor" id="2">
0x00
<calibration type="offset">
351.06 2674.02 -2772.45
</calibration>
<calibration type="matrix">
1.076917 0.072891 -0.013270
0.072891 0.960695 -0.023474
-0.013270 -0.023474 0.972254
</calibration>
</K_Ceptor>
</Branch>
<Branch Name="Brazo" id="3">CH_3<K_Ceptor Name="BrazoSensor" id="4">
0x00
<calibration type="offset">
236.99 -552.77 683.68
</calibration>
<calibration type="matrix">
0.978881 -0.013358 -0.021316
-0.013358 0.990111 -0.020486
-0.021316 -0.020486 1.032867
</calibration>
</K_Ceptor>
</Branch>
<Branch Name="Mano" id="5">CH_2<K_Ceptor Name="ManoSensor" id="5">
0x00
<calibration type="offset">
-904.09 873.90 1275.87
</calibration>
<calibration type="matrix">
9.3938e-01 -3.6198e-02 -2.6104e-02
-3.6198e-02 1.0290e+00 -6.5024e-04
-2.6104e-02 -6.5024e-04 1.0366e+00
</calibration>
</K_Ceptor>
</Branch>
</Mux>
<!-- <Mux Name="MAIN" id="0">
0XBC
<Branch Name="B-HEAD" id="1">
<K_Ceptor Name="L_arm" id="3">
0x01
<K_Ceptor Name="L_forarm" id="4">
0x02
<K_Ceptor Name="L_hand" id="5">
0x03
<Mux Name="SEC" id="0">
<Branch Name="B-HAND-BRANCH1" id="6">
<K_Ceptor Name="L_finger 1" id="7">
0x04
</K_Ceptor>
</Branch>
<Branch Name="B-HAND-BRANCH 2 " id="6">
<K_Ceptor Name="L_finger 2" id="7">
0x04
</K_Ceptor>
</Branch>
</Mux>
</K_Ceptor>
</K_Ceptor>
</K_Ceptor>
</Branch>
<Branch Name="B-FEET" id="6">
<K_Ceptor Name="L_foot" id="7">
0x04
</K_Ceptor>
</Branch>
</Mux>-->
</Armature>
</Chordata>
\ No newline at end of file
This source diff could not be displayed because it is too large. You can view the blob instead.
# NOTOCHORD
_Hub software of the Chordata motion capture system_
## ATTENTION: WIP!
This software is still on an BETA stage and under heavy development. Refer to the `develop` branch for the lastest version.
Current version `0.1.1b`.
For more information take a look at our [wiki](http://wiki.chordata.cc) or [forum](http://forum.chordata.cc)
This software is still on **BETA stage** and under heavy development. Refer to the `develop` branch for the lastest version.
### Hardware
Tested under a Raspberry Pi3 with Raspbian "Stretch" Lite
with one Chordata Hub R1.2 as a multiplexer
and three Chordata K-Ceptor R2.2 as sensors
### WIRING
#### Hub
To wire the Rasperry and Chordata Hub see [this section from our wiki](https://wiki.chordata.cc/wiki/User_Manual/2._Setting_up_the_system#Wiring_the_Hub_and_raspberry)
#### K-Ceptors
_The following connections are made with a 6 cores cable, ~26 AWG, with crimped RJ12 connectors._
- `[MUX J1] -> [First KCeptor IN] <Translation value #0>`
- `[First KCeptor OUT] -> [Second KCeptor IN] <Translation value #1>`
- `[Second KCeptor OUT]-> [Third KCeptor IN] <Translation value #2>`
For this to work the `Chordata.xml` file has to have the following structure):
```xml
<Chordata version="0.1.0">
<Configuration>
<KC_revision> 2 </KC_revision>
(... config nodes ...)
</Configuration>
<Armature>
<Mux Name="main_mux" id="0">
0x77
<Branch Name="main_branch" id="1">
CH_1
<K_Ceptor Name="first" id="2">
0
<K_Ceptor Name="second" id="3">
1
<K_Ceptor Name="third" id="4">
2
</K_Ceptor>
</K_Ceptor>
</K_Ceptor>
</Branch>
</Mux>
</Armature>
</Chordata>
```
Note that any other arbitrary hierarchy can be used, more info on [this section from our wiki](https://wiki.chordata.cc/wiki/User_Manual/3._Attaching_the_Hardware_to_the_performer%E2%80%99s_body).
### INSTALLATION
- **First of all you have to activate the i2c adaptor on your rPi**
```
sudo raspi-config
```
And activate the option under [5 Interfacing options] > [P5 I2C]
- **Then you have to fetch the dependencies, and compile**
The script `install_notochord.sh` will take care of cloning this repository, installing dependencies and do a minimal network configuration on your raspbian.
It can be executed with this one-liner:
```bash
bash <(curl -sL https://gitlab.com/chordata/notochord/raw/develop/install_notochord.sh)
```
- **You can now run the notochord**
```
cd bin
./notochord <YOUR COMPUTER IP ADDRESS>
```
### CALIBRATION
Before using each K-Ceptor it has to be calibrated. See [this section from our wiki](https://wiki.chordata.cc/wiki/User_Manual/4._Sensor_calibration) for more info.
The proccesing of the raw data is performed by a matlab script created by _Ozan AKTAÞ_: `calib/getMagCalib.m`
### LICENSE
This program is free software and is distributed under the GNU General Public License, version 3.
You are free to run, study, share and modify this software.
Derivative work can only be distributed under the same license terms, and replicate this program copyright notice.
More info [here](https://www.gnu.org/licenses/gpl-faq.en.html).
Derivative work can only be distributed under the same license terms, and replicating this program copyright notice.
More info about the terms of the license [here](https://www.gnu.org/licenses/gpl-faq.en.html).
\ No newline at end of file
......@@ -6,6 +6,9 @@ Hub software of the Chordata motion capture system
import scons_utils as utils
from os import mkdir
from os import environ
from shutil import copyfile
cmd_line_compiler = ARGUMENTS.get('compiler', 0)
vc = utils.Version_Check(cmd_line_compiler)
......@@ -14,6 +17,7 @@ vc = utils.Version_Check(cmd_line_compiler)
vars = Variables(None, ARGUMENTS)
vars.Add('compiler', 'Pass the name or path of the compiler to be used\n A comma separated list can also be passed ', vc.compilers, 0)
vars.Add('debug', 'Set to 1 in order make a debug compilation ( -g flag ) ', 0)
# vars.Add('i2c_support', 'Set to 0 in order make dummy compilation with no real i2c ', 1)
vars.Add('test', "Set to 1 in order to compile the test suit", 0)
vars.Add('permisive', "Set to 1 in order to compile the test with -fpermissive", 0)
......@@ -23,31 +27,47 @@ if not GetOption('help'):
env = Environment(
CXX= compiler,
CCFLAGS= ['-std=c++14'],
CPPPATH=['#lib', '#lib/oscpack_1_1_0','#lib/MadgwickAHRS', '#lib/tinyxml2'])
CCFLAGS= ['-std=c++14', "-pthread", "-w", "-Wno-deprecated"],
CPPPATH=['#lib', '#src', '#lib/oscpack_1_1_0','#lib/MadgwickAHRS', '#lib/tinyxml2'],
# CPPDEFINES = ["FMT_HEADER_ONLY"]
)
# Pass the terminal to the environment in order to let Scons check if it's color-capable
env['ENV']['TERM'] = environ['TERM']
try:
env['ENV']['TERM'] = environ['TERM']
except Exception as e:
print "Couldn't determinate $TERM env variable"
# if not GetOption('i2c_support'):
# env.Append(CPPDEFINES= ["__NO_I2C_SUPPORT__"])
debug = ARGUMENTS.get('debug', 0)
if int(debug):
env.Append(CCFLAGS = ['-g'])
_libs = SConscript(['lib/SConscript'], exports = 'env')
_chordlib = SConscript(['src/SConscript'], exports = 'env')
_libs = _chordlib + SConscript(['lib/SConscript'], exports = 'env')
test = ARGUMENTS.get("test")
if not test:
env.Append( CPPDEFINES = ["__CHORDATA_MAIN__"])
try:
mkdir("bin")
except Exception as e:
pass
copyfile("calib/getMagCalib.m", "bin/getMagCalib.m")
env.Append(CPPPATH = ["/usr/local/include/octave-4.0.0/octave", "/usr/include/octave-4.0.0/octave", "/usr/include/octave-4.0.3/octave", "/usr/include/octave-4.2.2/octave"])
env.Program("#bin/notochord", "src/Notochord.cpp",
LIBS = _libs
LIBPATH = ["/usr/lib/x86_64-linux-gnu", "/usr/lib/arm-linux-gnueabihf"],
LIBS = _libs + ["pthread", "octinterp", "octave"]
)
else:
env.Append(CPPPATH = ["#test"])
env.Append(CCFLAGS = ["-pthread"])
env.Append(CPPDEFINES = ["_CHORDATA_TEST_BUILD__"])
if GetOption('help'):
env.Append(CCFLAGS = ["-fpermissive"])
......
#!/usr/bin/python
import sys
#print 'Number of arguments:', len(sys.argv), 'arguments.'
#print 'Argument List:', str(sys.argv)
if len(sys.argv) > 1:
val = sys.argv[1]
print "[ XM ] XOR 0x6b: {0} ( 0x{0:02X} )".format(int(val, 16) ^ 0x6b)
print "[ G ] XOR 0x1e: {0} ( 0x{0:02X} )".format(int(val, 16) ^ 0x1e)
else:
print "give a value to have it XORed with 0x6b, 0x1d (LSM9DS1 adresses))"
function [ Bfield, offset, W_inverted ,foo, bar, correctedM] = ellipsoid_fit2magnetic_data(M)
%
% Fit an ellispoid to a set of xyz data points
%compansate hard-iron, soft-iron and scaling effects.
% Parameters:
% M - Cartesian data, n x 3 matrix (Containing magnetic data mx,my,mz as columns)
% Output:
% Bfield - Radius of sphere having same volume with elipsoid fitted to data
% offset - ellispoid center coordinates [xc; yc; zc](used for Hard-iron effect compansation)
%W_inverted - Matrix needed to transform data (vector) on the elipsoidal
%surface to a spherical surface (used for Soft-Iron effect and scaling/calibration error compansation)
%Definitions
% * radii - ellipsoid radii [a; b; c]
% * evecs - ellipsoid radii directions as columns of the 3x3 matrix
% * v - the 9 parameters describing the ellipsoid algebraically:
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
% Author:
% Ozan AKTA?
% Turkey, Ankara
%
x = M( :, 1 );
y = M( :, 2 );
z = M( :, 3 );
D = [ x .* x, ...
y .* y, ...
z .* z, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z ]; % n data points x 9 ellipsoid parameters
% solve the normal system of equations and find fitted ellipsoid parameters
v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
% form the algebraic form of the ellipsoid
A = [ v(1) v(4) v(5) v(7); ...
v(4) v(2) v(6) v(8); ...
v(5) v(6) v(3) v(9); ...
v(7) v(8) v(9) -1 ];
% find the center of the ellipsoid
offset = A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
%remove offset, do same calculation again written in a simplified algebraic form of elipsoid
x = x+offset(1);
y = y+offset(2);
z = z+offset(3);
K = [ x .* x, ...
y .* y, ...
z .* z, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z]; %
% solve the normal system of equations
p = ( K' * K ) \ ( K' * ones( size( x, 1 ), 1 ) );
% form the algebraic form of the ellipsoid with center at (0,0,0)
AA = [ p(1) p(4) p(5) ; ...
p(4) p(2) p(6) ; ...
p(5) p(6) p(3) ];
% solve the eigenproblem
[ evecs evals ] = eig( AA );
radii = sqrt( 1 ./ diag( evals ) );
%calculate radius of sphere having same volume with the elipsoid
Bfield=(radii(1)*radii(2)*radii(3))^(1/3);
%calculate transformation matrix elipsoidal to spherical
W_inverted=evecs*sqrt(evals)*inv(evecs)*Bfield;
correctedM=W_inverted*[x';y';z'];
foo = [x,y,z];
bar = [x';y';z'];
% % draw raw data and corrected data
% figure;
% hold on;
% grid on;
% %plot corrected data with offset and eccentricity removed
% plot3( correctedM(1,:),correctedM(2,:) , correctedM(3,:),'ro' );
% %Plot raw data with offset removed (for visual purposes).
% plot3( x-offset(1), y-offset(2), z-offset(3) );
% plot3( x, y, z, 'g' );
% %draw elipsoidal fit to raw data with offset removed
% maxd = max( [ radii(1) radii(2) radii(3)] );
% step = maxd / 50;
% [ xx, yy, zz ] = meshgrid( -maxd:step:maxd, -maxd:step:maxd, -maxd:step:maxd );
% Ellipsoid = p(1) *xx.*xx + p(2) * yy.*yy + p(3) * zz.*zz + ...
% 2*p(4) *xx.*yy + 2*p(5)*xx.*zz + 2*p(6) * yy.*zz ;
% isosurface( xx, yy, zz, Ellipsoid, 1 );
% alpha(.4);
% axis square
% view([-45 45]);
\ No newline at end of file
function [ Bfield, offset, W_inverted ] = getMagCalib(M)
%
% Fit an ellispoid to a set of xyz data points
%compansate hard-iron, soft-iron and scaling effects.
% Parameters:
% M - Cartesian data, n x 3 matrix (Containing magnetic data mx,my,mz as columns)
% Output:
% Bfield - Radius of sphere having same volume with elipsoid fitted to data
% offset - ellispoid center coordinates [xc; yc; zc](used for Hard-iron effect compansation)
%W_inverted - Matrix needed to transform data (vector) on the elipsoidal
%surface to a spherical surface (used for Soft-Iron effect and scaling/calibration error compansation)
%Definitions
% * radii - ellipsoid radii [a; b; c]
% * evecs - ellipsoid radii directions as columns of the 3x3 matrix
% * v - the 9 parameters describing the ellipsoid algebraically:
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
% Author:
% Ozan AKTAÞ
% Turkey, Ankara
%
x = M( :, 1 );
y = M( :, 2 );
z = M( :, 3 );
originx = x;
originy = y;
originz = z;
D = [ x .* x, ...
y .* y, ...
z .* z, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z ]; % n data points x 9 ellipsoid parameters
% solve the normal system of equations and find fitted ellipsoid parameters
v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
% form the algebraic form of the ellipsoid
A = [ v(1) v(4) v(5) v(7); ...
v(4) v(2) v(6) v(8); ...
v(5) v(6) v(3) v(9); ...
v(7) v(8) v(9) -1 ];
% find the center of the ellipsoid
offset = A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
%remove offset, do same calculation again written in a simplified algebraic form of elipsoid
x = x+offset(1);
y = y+offset(2);
z = z+offset(3);