comes from the accelerometer, 
is velocity, and 
is position.Pose: An Arm Tracking System
Daryl Sew Cornell CS ‘17 darylsew@gmail.com
Emma Wang Cornell ECE ‘17 exw2@cornell.edu
Zachary Zimmerman Cornell CS + Game Design ‘17 ztz3@cornell.edu
References and Acknowledgements
KXG03 - gyro-accel sensor unit
We created an arm mounted sensor array which interfaces with a PIC32 unit to produce odometry data for arm motion tracking.
We were all interested in wearable technology, virtual reality, and sensor fusion, so we designed a project that drew from all of these fields. We wanted to create a device that would allow for an immersive martial arts video gaming experience, or evaluation of form and posture for fitness applications.
Body pose tracking has many applications within gaming, healthcare, and other industries. A low cost, effective solution to this problem could easily disrupt the existing high end systems that are used for motion tracking, so the opportunity for either commercialization or publication also appeals to us.
We used Inertial Measurement Units (IMUs) made of gyroscope-accelerometer and magnetometer-accelerometer sensor units along three points of the arm to measure accelerations, angular velocities, and magnetic field vectors. While the sensors gather data on three axes, the PIC32 retrieves their register contents via I2C communication. The PIC32 then communicates sensor readings over serial to a PC which integrates the readings, applys filtering algorithms to produce 3D positions and orientations. These are then used as inputs into a virtual reality application.
We considered several different overall approaches to body pose tracking, such as computer vision, LIDAR, structured light, and flex sensor based approaches. We decided to use gyroscopes, accelerometers, and magnetometers as these components are extremely low cost and can easily be integrated into a wearable device; desirable features for the consumer applications we have in mind. We anticipated that we would be able to apply advanced sensor fusion techniques to bring the odometry quality up to par with higher cost, higher fidelity sensors.
Our project deliverable does not have standards to which it adheres. However, we found the following IEEE standards helpful, as they enabled compatibility between our disparate devices.
P1780 - Standard for the Specification of Inertial Measurement Units (IMU)
P1394 - Standard for a High-Performance Serial Bus
We used Kionix provided tri-axis accelerometers to measure the acceleration in x, y, and z axes at a particular point on an arm. We integrate this acceleration to produce velocity, and integrate velocity to produce position, according to the following 3D kinematic equations. The value for comes from the accelerometer, is velocity, and is position.
Our accelerometer senses acceleration via “differential capacitance arising from acceleration induced motion of the sense element” (see KXG03 datasheet). Electromagnetic interference from circuitry can affect the quality of the data, so common mode cancellation is used to compensate for this. Additionally, temperature can also change the sensor readings, so the accelerometer has a temperature sensor and a DSP which applies temperature compensation as well as some basic filtering techniques.
We used Kionix provided tri-axis gyroscopes to measure the angular velocities in x, y and z axes at a particular point on an arm. We then integrate these velocities to produce an orientation, which we represent using quaternions in order to avoid the singularities of an Euler angle orientation representation (see Gimbal Lock), as well as to allow for smooth interpolation of data and provide more effective incremental behavior.
Gyroscopes measure accelerations via capacitance changes at sensor electrodes; see the KXG03 datasheet for more information.
We used Kionix provided tri-axis magnetometers to detect the direction of the magnetic field at a particular point on an arm. As this direction typically points in the direction of Earth’s magnetic field (a constant), it can be used as an anchor point for orientation to avoid gyroscope drift.
Magnetometers measure accelerations via magnetic impedance measurements on a “special electron spin aligned amorphous wire.” See the KMX62 datasheet for more information.
In the field of robotic manipulation, forward kinematics refers to the use of a model of an arm as a sequence of links (translations) and joints (rotations) to, given joint angles, produce the position of the end effector. Using translation matrices to represent a link (i.e. a forearm) and rotation matrices to represent a joint (i.e. an elbow), we can compose these matrices to produce an output matrix which represents the end effector pose.
We use forward kinematics in our virtual reality application to compute joint and end effector position given joint angles from filtered gyroscope data.
There are many patents for designs similar to ours. The following list contains several patents which we consider most relevant.
To the best of our knowledge, none of these other designs have used the same fusion techniques and sensors as we have though, or used the body pose as input into the same virtual reality application we have created.
In order to read data as quickly as possible from the PIC32, we decided to move as much of the computation as possible off the microcontroller to a full powered computer by having the PIC32 communicate sensor data over serial. This also enables us to use a powerful but CPU intensive 3D graphics engine which we would have been unable to do on the PIC32 with the TFT display.
In using the I2C bus, we were initially under the impression that we would be able to open one channel and connect all six sensor units (3 gyro-accel, 3 mag-accel) to it with each at its unique address. However, we discovered that both sensor unit designs only allowed I2C user addressing of one bit, resulting in only 2 unique I2C address per type of sensor unit. Hence, we could hook up at most 2 gyro-accel and 2 mag-accel units on the one I2C channel.
To retrieve data from the third set of sensor units we next tried opening the second I2C channel on the PIC32, which unfortunately resulted in transmission from both channels stalling. Instead of spending more time debugging this issue, we moved on to an alternative solution.
By blasting Read/Write instructions to the same address for either gyrometer-accelerometer or magnetometer-accelerometer and toggling the ADDR line of each respective sensor unit, we were able to implement a Chip-Select function on one I2C channel. There were no timing issues with toggling the ADDR line across each sensor unit, and thus successful communication was established amongst three sensor units.
We had two main chunks of software. One, in C code, was compiled and loaded onto the PIC32 and was responsible for reading sensor data over I2C and sending it to a computer over serial. The other one, in Python, was responsible for parsing incoming data on the serial port, filtering/fusing it, integrating it, and rendering it via the game library Panda3D.
We set the baud rate to 115200 here in order to communicate over serial as quickly as possible. If we had more time, we would have tried to enable a higher speed mode; this was the highest that worked without changing anything else.
In this file, based on the sensor datasheets and PLIB manual, we implemented i2c_read and i2c_write wrapper functions to abstract away reading or writing to a register on a sensor.
In our main C file, we had a single thread which started out by writing configuration and wakeup values to our sensors, then repeatedly reading from sensors and sending data over serial.
The protocol we designed for sending data over serial used spaces to delimit registers from the same sensor, newlines to delimit different sensors, and the word ‘init’ followed by a newline to delimit sensor readings at different timesteps. This protocol enables the corresponding Python script to connect to the device at any time as the script can use inits to separate out sensor readings by timestamp.
The following is an example reading from 1 timestep, where the first 3 lines are angular velocity and acceleration readings from gyroscopes, and the last three lines are magnetic field and acceleration readings from magnetometers.
init
6 34 8 -899 -14546 -9824
32511 40 28009 9119 -867 11209
36 -259 -261 -3954 -2928 14885
-1944 34 1280 514 12810 14885
2343 -2008 -126 3392 256 7179
2048 -1623 262 -10624 1024 4110
In this file, we use pyserial to read incoming data on the serial port and parse it into custom classes for accelerometer data and gyroscope data. In the read loop, we integrate the data and update an odometry dictionary, which keeps track of the pose and velocity of each sensor. If run as main, this file prints out the sensor values it is reading. We fuse the data by averaging accelerations from the two accelerometers we have at each mount point; as the error sources are uncorrelated in time between the two, averaging data should produce more accurate readings. We also apply a complementary filter to the orientation, where the angular velocity is replaced by 0.98 * angular_velocity + 0.02 * acceleration. This enables us to eliminate gyroscope drift by deferring to the accelerometer gravity vector at low angular velocity (i.e. stationary) speeds.
For each sensor, we take the average or median of the first 120 sensor readings and in future readings, use that value as an offset. We require that the devices are at rest during this time.
In this file, we enable concurrent execution of the game and reading from serial in a while True loop. Thanks to the global interpreter lock, the only way to be truly concurrent in Python is to use process concurrency through interprocess communication. As such, we implemented a shared queue that the read_sensors.py process uses to communicate odometry data over to the process rendering the game.
We used this to create a 3D scatter plot.
In this file, we create a virtual representation of an arm in a sandbox environment, allowing the user to interact with objects on the screen just by moving their arm. The game we made to demonstrate these capabilities lets the user hit balls and watch them bounce around a ball pit according to physics simulation.
We used several Python libraries to speed up the software development process.
These libraries are all BSD compatible, so we are free to use them so long as we include a BSD license, which we have.
We also referred to several blog posts and websites in order to understand how to properly calibrate the devices, and to survey different filtering algorithms, but we did not take any code from these websites. See References for more information.
Initially, we did not attempt to calibrate or fuse any of the devices, which resulted in extremely poor position and orientation estimates. With higher quality sensors, we might have been able to get sufficient accuracy and precision off just the raw readings, thanks to each sensor’s built-in correction, fusion and filtering algorithms.
We intended to use the magnetometers to cancel out orientation drift in the gyroscope, but despite our best efforts in calibrating them, we found their readings to be inconsistent and noisy, so unfortunately we did not use magnetometers in the final software package.
We also intended to use integrated position information from the accelerometers, but found that position estimates accumulated error extremely quickly, despite our best calibration and fusion efforts. We think that with a Kalman filter we may be able to mitigate this, but were unable to get a good position reading for now. Our attempts to use a Kalman filter failed due to the complexity.
The KXG03 gyro-accel unit and KMX62 mag-accel unit work together to form the gyro-accel-mag IMU. The IMUs, or in this case sensor unit pairs, were placed along strategic mapping points along the arm; shoulder, elbow, hand (see figure). Each IMU received VDD, IO_VDD (same value as VDD), GND, I2C channel 1 clk/data lines and addressed last bit of I2C address to 1 or 0 depending on the output of the PIC32 pin assigned to the sensor unit (pin 4 to shoulder unit, pin 5 to elbow unit, pin 6 to hand unit). See Appendix C for detailed overall schematic.
In order to safely power the KMX62 and KXG03 sensors, we had to drive them with 2.6V rather than the 3.3V the PIC provides. We achieved this using a diode to step down the voltage and grounding this through a 1kΩ resistor, 10μF and 0.1μF capacitors as seen here.
In addition, the address pin “chip select” lines needed to be 2.6V as well, for these we created a voltage divider with 100Ω and 330Ω resistors to create the voltage
These voltage dividers were connected to the output pins for the chip select as seen here.
Following the pinout and schematic from the KMX62 and KXG03, we wired each unit so that minimal wires were used. This involved implementing 1-to-2 and 1-to-3 splitters by soldering jumpers along each junction. We used ribbon wire wherever possible to contain wires and minimize jostling. All of our hardware design is original.
Since we are using donated Kionix sensors, we thought it would be simple to request factory-set uniquely addressed sensor units. This way with all sensors uniquely addressed, they would be connected to one I2C channel, cutting down on wire length. However, after much back and forth with company contacts, it was clear that it would not be realistic for us to wait until the company assembly could be changed for our device request. As mentioned previously, after a process of finding the best way to wire the hardware, we settled on implementing a Chip Select to retrieve data from the three sensor units from one I2C channel.
We implemented a calibration period of three seconds over which we ask the human subject to extend their arm horizontally outward with all three IMUs facing upwards. The data we collect during calibration is averaged as used for initial values after which we can integrate for relative changes in position and orientation. The calibration offsets differ depending on the location and temperature, but the following are sample calibration offsets; the first three lines are gyroscope offsets, and last three lines are magnetometer offsets. The first three readings per line are angular velocity / magnetic field, last three are accelerations.
6 34 8 -899 -14546 -9824
32511 40 28009 9119 -867 11209
36 -259 -261 -3954 -2928 14885
-1944 34 1280 514 12810 14885
2343 -2008 -126 3392 256 7179
2048 -1623 262 -10624 1024 4110
In an attempt to calibrate our magnetometers, we moved it around during this calibration period and plotted the resulting readings, which should have resembled a hollow sphere. However, the results were not quite spherical, as follows, meaning the centroid would not have made for a good magnetometer calibration value. We unfortunately did not find the calibrated magnetometer data to be useful.
We enforced safety in our design through relying on the user to limit their maximum velocity and acceleration while using the device so they couldn’t hit someone accidentally. We also ensured that the area around users was clear during testing, and we made comfortable felt straps so that wearing the device would not cause injury due to rubber bands. In general as it is a passive measurement device, there is little safety risk.
With a baud rate of 115200, we were able to read from all 6 sensors at approximately 41Hz. Enabling a higher speed rate of transfer could possibly result in higher quality odometry, as a higher quantity of sensor data would enable us to improve our fusion algorithms.
We found that a complementary filter substantially increased the overall usability of our application. We went from having arm drift from the beginning (0 seconds) to being able to run for up to 2 minutes before having to reset to eliminate drift. If we had good ground truth measurements for arm motion (say a track the arm follows that we machine), then we would be able to provide a more quantitative analysis of our accuracy.
Our demo from class shows the technology off pretty well, and can be seen here.
We are pleased with the state of the project, as the device successfully enabled users to manipulate objects in a virtual world by moving their arm around. The complementary filter was very effective in eliminating orientation drift, producing orientation measurements that were impressively precise.
However, we are unhappy that we had to throw away position information; there were extreme amounts of drift due to accumulated acceleration error. If we had more time, we would look into building a better model of the system dynamics and using a Kalman filter to improve our position reading quality, and playing around with different approaches to fusing the data.
We also wish we had been able to make better use of the magnetometers, as we found the data from them was too low quality to be used for eliminating gyroscopic drift. A better calibration procedure or different setup options may have enabled us to use them more effectively, or possibly advanced sensor fusion algorithms.
We did not use a previous third party design. We relied on our existing experience and skills learned in this class as well as creativity to create this original project.
We used the i2c_helper.h function from a previous project (Self Balancing Robot 2015) as a guideline for writing our own I2C functions and referred to the same group’s code to understand the complementary filter. We also pulled Python libraries in public domain to use their functions in our code.
Thanks to Kionix for donating sensors.
Patent opportunities exist for this project and would require further research. Currently our project and project website are available for online viewing. We are looking into opportunities for publishing a conference or workshop paper.
We followed all ethical requirements as listed in the IEEE code of ethics. Though our project tracks the user’s arm motion, we do not collect any data from the user’s motions and the user receives direct feedback from the screen about their arm movement. We further made the apparatus comfortable to wear by fitting each sensor unit with comfortable bands and smooth surfaces to minimize sensitivity and reduce discomfort. When we did solder, we used appropriate protection and precautions to minimize risk.
There are none we are aware of, as we used wired communication only.
If this material helped you with your research, please consider listing us as authors of your publication (see contact information). In the future, we intend to continue this work and publish it. At that point, we will update this section with citation information for our paper.
We’re releasing the source code to this project under the BSD 2 Clause License, as follows.
Copyright (c) <2016>, <Daryl Sew, Emma Wang, Zachary Zimmerman>
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
The group approves this report for inclusion on the course website. The group approves the video for inclusion on the course YouTube channel.
/*
* File: config.h
* Author: Syed Tahmid Mahbub
* Modifed by: Bruce Land
* Created on October 10, 2014
* Mod: 24Sept2015
*/
#ifndef CONFIG_H
#define CONFIG_H
#define _SUPPRESS_PLIB_WARNING 1
#include <plib.h>
// serial stuff
#include <stdio.h>
//=============================================================
// 40 MHz
#pragma config FNOSC = FRCPLL, POSCMOD = OFF
#pragma config FPLLIDIV = DIV_2, FPLLMUL = MUL_20 //40 MHz
#pragma config FPBDIV = DIV_1, FPLLODIV = DIV_2 // PB 40 MHz
#pragma config FWDTEN = OFF, FSOSCEN = OFF, JTAGEN = OFF
//==============================================================
// Protothreads configure
// IF use_vref_debug IS defined, pin 25 is Vref output
//#define use_vref_debug
// IF use_uart_serial IS defined, pin 21 and pin 22 are used by the uart
#define use_uart_serial
//#define BAUDRATE 9600 // must match PC terminal emulator setting
//#define BAUDRATE 14400 // must match PC terminal emulator setting
// woo super high baud rate
#define BAUDRATE 115200
/////////////////////////////////
// set up clock parameters
// system cpu clock
#define sys_clock 40000000
// sys_clock/FPBDIV
#define pb_clock sys_clock // divide by one in this case
#endif /* CONFIG_H */
#define USE_AND_OR // To enable AND_OR mask setting for I2C.
//#include <i2c.h>
#include "plib.h"
#include "tft_master.h"
//#define SLAVE_ADDRESS 0xd0
#define GYRO_ACCEL_XOUT_L 0x08
#define GYRO_ACCEL_XOUT_H 0x09
#define GYRO_ACCEL_YOUT_L 0x0A
#define GYRO_ACCEL_YOUT_H 0x0B
#define GYRO_ACCEL_ZOUT_L 0x0C
#define GYRO_ACCEL_ZOUT_H 0x0D
#define GYRO_XOUT_L 0x02
#define GYRO_XOUT_H 0x03
#define GYRO_YOUT_L 0x04
#define GYRO_YOUT_H 0x05
#define GYRO_ZOUT_L 0x06
#define GYRO_ZOUT_H 0x07
#define MAG_ACCEL_XOUT_L 0x0A
#define MAG_ACCEL_XOUT_H 0x0B
#define MAG_ACCEL_YOUT_L 0x0C
#define MAG_ACCEL_YOUT_H 0x0D
#define MAG_ACCEL_ZOUT_L 0x0E
#define MAG_ACCEL_ZOUT_H 0x0F
#define MAG_XOUT_L 0x10
#define MAG_XOUT_H 0x11
#define MAG_YOUT_L 0x12
#define MAG_YOUT_H 0x13
#define MAG_ZOUT_L 0x14
#define MAG_ZOUT_H 0x15
#define GYRO_SCALE 131.0 // lsb/(degrees/second)
// bit 1: 0 for gyro, 1 for magnetometer
#define KIONIX_MAG_W1 0x1E
#define KIONIX_MAG_R1 0x1F
#define KIONIX_MAG_W2 0x1C
#define KIONIX_MAG_R2 0x1D
#define KIONIX_GYRO_W1 0x9C
#define KIONIX_GYRO_R1 0x9D
#define KIONIX_GYRO_W2 0x9E
#define KIONIX_GYRO_R2 0x9F
// i2c hack: we only have one address for each thingy
#define KIONIX_MAG_WRITE 0x1C
#define KIONIX_MAG_READ 0x1D
#define KIONIX_GYRO_WRITE 0x9E
#define KIONIX_GYRO_READ 0x9F
float X_GYRO_OFF, Y_GYRO_OFF, Z_GYRO_OFF;
// Wait by executing nops
void i2c_wait(unsigned int cnt)
{
while(--cnt)
{
asm( "nop" );
asm( "nop" );
}
}
/**
* This file contains lots of i2c wrapper stuff. Pretty simple and convenient.
*/
void i2c_write(char device_address, char register_address, char *data, int data_len) {
// Send the Start Bit
StartI2C1();
IdleI2C1();
// Write the slave address
MasterWriteI2C1(device_address);
IdleI2C1();
while (I2C1STATbits.ACKSTAT) {}
MasterWriteI2C1(register_address);
while (I2C1STATbits.ACKSTAT) {}
IdleI2C1();
int i;
for (i = 0; i < data_len; i++) {
MasterWriteI2C1(data[i]);
IdleI2C1();
while (I2C1STATbits.ACKSTAT) {}
i++;
}
StopI2C1(); // Send the Stop condition
IdleI2C1(); // Wait to complete
}
void i2c_write_shoulder(char device_address, char register_address, char *data, int data_len) {
mPORTASetBits(BIT_2);
i2c_write(device_address, register_address, data, data_len);
mPORTAClearBits(BIT_2);
}
void i2c_write_elbow(char device_address, char register_address, char *data, int data_len) {
mPORTASetBits(BIT_3);
i2c_write(device_address, register_address, data, data_len);
mPORTAClearBits(BIT_3);
}
void i2c_write_wrist(char device_address, char register_address, char *data, int data_len) {
mPORTASetBits(BIT_4);
i2c_write(device_address, register_address, data, data_len);
mPORTAClearBits(BIT_4);
}
void i2c2_write(char device_address, char register_address, char *data, int data_len) {
// Send the Start Bit
StartI2C2();
IdleI2C2();
// Write the slave address
MasterWriteI2C2(device_address);
IdleI2C2();
while (I2C2STATbits.ACKSTAT) {}
MasterWriteI2C2(register_address);
while (I2C2STATbits.ACKSTAT) {}
IdleI2C2();
int i;
for (i = 0; i < data_len; i++) {
MasterWriteI2C2(data[i]);
IdleI2C2();
while (I2C2STATbits.ACKSTAT) {}
i++;
}
StopI2C2(); // Send the Stop condition
IdleI2C2(); // Wait to complete
}
char i2c_read(char write_address, char read_address,
char register_address) {
StartI2C1(); // Send the Start Bit
IdleI2C1(); // Wait to complete
MasterWriteI2C1(write_address);
IdleI2C1();
while (I2C1STATbits.ACKSTAT) {}
MasterWriteI2C1(register_address);
IdleI2C1();
while (I2C1STATbits.ACKSTAT) {}
// now send a start sequence again
RestartI2C1(); // Send the Restart condition
i2c_wait(10);
// wait for this bit to go back to zero
IdleI2C1(); // Wait to complete
MasterWriteI2C1(read_address); // transmit read command
IdleI2C1(); // Wait to complete
while (I2C1STATbits.ACKSTAT) {}
char data = MasterReadI2C1();
IdleI2C1(); // Wait to complete
NotAckI2C1();
StopI2C1(); // Send the Stop condition
IdleI2C1(); // Wait to complete
return data;
}
char i2c_read_shoulder(char write_address, char read_address, char register_address) {
mPORTASetBits(BIT_2);
char data = i2c_read(write_address, read_address, register_address);
mPORTAClearBits(BIT_2);
return data;
}
char i2c_read_elbow(char write_address, char read_address, char register_address) {
mPORTASetBits(BIT_3);
char data = i2c_read(write_address, read_address, register_address);
mPORTAClearBits(BIT_3);
return data;
}
char i2c_read_wrist(char write_address, char read_address, char register_address) {
mPORTASetBits(BIT_4);
char data = i2c_read(write_address, read_address, register_address);
mPORTAClearBits(BIT_4);
return data;
}
char i2c2_read(char write_address, char read_address,
char register_address) {
StartI2C2(); // Send the Start Bit
IdleI2C2(); // Wait to complete
MasterWriteI2C2(write_address);
IdleI2C2();
while (I2C2STATbits.ACKSTAT) {}
MasterWriteI2C2(register_address);
IdleI2C2();
while (I2C2STATbits.ACKSTAT) {}
// now send a start sequence again
RestartI2C2(); // Send the Restart condition
i2c_wait(10);
// wait for this bit to go back to zero
IdleI2C2(); // Wait to complete
MasterWriteI2C2(read_address); // transmit read command
IdleI2C2(); // Wait to complete
while (I2C2STATbits.ACKSTAT) {}
char data = MasterReadI2C2();
IdleI2C2(); // Wait to complete
NotAckI2C2();
StopI2C2(); // Send the Stop condition
IdleI2C2(); // Wait to complete
return data;
}
void read_gyro_values(char write_address, char read_address, int *values) {
int xGyroH = (int)i2c_read(write_address, read_address, GYRO_XOUT_H);
int xGyroL = (int)i2c_read(write_address, read_address, GYRO_XOUT_L);
int yGyroH = (int)i2c_read(write_address, read_address, GYRO_YOUT_H);
int yGyroL = (int)i2c_read(write_address, read_address, GYRO_YOUT_L);
int zGyroH = (int)i2c_read(write_address, read_address, GYRO_ZOUT_H);
int zGyroL = (int)i2c_read(write_address, read_address, GYRO_ZOUT_L);
values[0] = (xGyroH << 8) + xGyroL;
values[1] = (yGyroH << 8) + yGyroL;
values[2] = (zGyroH << 8) + zGyroL;
int xAccelH = (int)i2c_read(write_address, read_address, GYRO_ACCEL_XOUT_H);
int xAccelL = (int)i2c_read(write_address, read_address, GYRO_ACCEL_XOUT_L);
int yAccelH = (int)i2c_read(write_address, read_address, GYRO_ACCEL_YOUT_H);
int yAccelL = (int)i2c_read(write_address, read_address, GYRO_ACCEL_YOUT_L);
int zAccelH = (int)i2c_read(write_address, read_address, GYRO_ACCEL_ZOUT_H);
int zAccelL = (int)i2c_read(write_address, read_address, GYRO_ACCEL_ZOUT_L);
values[3] = (xAccelH << 8) + xAccelL;
values[4] = (yAccelH << 8) + yAccelL;
values[5] = (zAccelH << 8) + zAccelL;
}
void read_gyro_values_shoulder(char write_address, char read_address, int *values) {
mPORTASetBits(BIT_2);
read_gyro_values(write_address, read_address, values);
mPORTAClearBits(BIT_2);
}
void read_gyro_values_elbow(char write_address, char read_address, int *values) {
mPORTASetBits(BIT_3);
read_gyro_values(write_address, read_address, values);
mPORTAClearBits(BIT_3);
}
void read_gyro_values_wrist(char write_address, char read_address, int *values) {
mPORTASetBits(BIT_4);
read_gyro_values(write_address, read_address, values);
mPORTAClearBits(BIT_4);
}
void read_gyro_values_i2c2(char write_address, char read_address, int *values) {
int xGyroH = (int)i2c2_read(write_address, read_address, GYRO_XOUT_H);
int xGyroL = (int)i2c2_read(write_address, read_address, GYRO_XOUT_L);
int yGyroH = (int)i2c2_read(write_address, read_address, GYRO_YOUT_H);
int yGyroL = (int)i2c2_read(write_address, read_address, GYRO_YOUT_L);
int zGyroH = (int)i2c2_read(write_address, read_address, GYRO_ZOUT_H);
int zGyroL = (int)i2c2_read(write_address, read_address, GYRO_ZOUT_L);
values[0] = (xGyroH << 8) + xGyroL;
values[1] = (yGyroH << 8) + yGyroL;
values[2] = (zGyroH << 8) + zGyroL;
int xAccelH = (int)i2c2_read(write_address, read_address, GYRO_ACCEL_XOUT_H);
int xAccelL = (int)i2c2_read(write_address, read_address, GYRO_ACCEL_XOUT_L);
int yAccelH = (int)i2c2_read(write_address, read_address, GYRO_ACCEL_YOUT_H);
int yAccelL = (int)i2c2_read(write_address, read_address, GYRO_ACCEL_YOUT_L);
int zAccelH = (int)i2c2_read(write_address, read_address, GYRO_ACCEL_ZOUT_H);
int zAccelL = (int)i2c2_read(write_address, read_address, GYRO_ACCEL_ZOUT_L);
values[3] = (xAccelH << 8) + xAccelL;
values[4] = (yAccelH << 8) + yAccelL;
values[5] = (zAccelH << 8) + zAccelL;
}
void read_mag_values(char write_address, char read_address, int *values) {
int xMagH = (int)i2c_read(write_address, read_address, MAG_XOUT_H);
int xMagL = (int)i2c_read(write_address, read_address, MAG_XOUT_L);
int yMagH = (int)i2c_read(write_address, read_address, MAG_YOUT_H);
int yMagL = (int)i2c_read(write_address, read_address, MAG_YOUT_L);
int zMagH = (int)i2c_read(write_address, read_address, MAG_ZOUT_H);
int zMagL = (int)i2c_read(write_address, read_address, MAG_ZOUT_L);
values[0] = (xMagH << 8) + xMagL;
values[1] = (yMagH << 8) + yMagL;
values[2] = (zMagH << 8) + zMagL;
int xAccelH = (int)i2c_read(write_address, read_address, MAG_ACCEL_XOUT_H);
int xAccelL = (int)i2c_read(write_address, read_address, MAG_ACCEL_XOUT_L);
int yAccelH = (int)i2c_read(write_address, read_address, MAG_ACCEL_YOUT_H);
int yAccelL = (int)i2c_read(write_address, read_address, MAG_ACCEL_YOUT_L);
int zAccelH = (int)i2c_read(write_address, read_address, MAG_ACCEL_ZOUT_H);
int zAccelL = (int)i2c_read(write_address, read_address, MAG_ACCEL_ZOUT_L);
values[3] = (xAccelH << 8) + xAccelL;
values[4] = (yAccelH << 8) + yAccelL;
values[5] = (zAccelH << 8) + zAccelL;
}
void read_mag_values_shoulder(char write_address, char read_address, int *values) {
mPORTASetBits(BIT_2);
read_mag_values(write_address, read_address, values);
mPORTAClearBits(BIT_2);
}
void read_mag_values_elbow(char write_address, char read_address, int *values) {
mPORTASetBits(BIT_3);
read_mag_values(write_address, read_address, values);
mPORTAClearBits(BIT_3);
}
void read_mag_values_wrist(char write_address, char read_address, int *values) {
mPORTASetBits(BIT_4);
read_mag_values(write_address, read_address, values);
mPORTAClearBits(BIT_4);
}
void read_mag_values_i2c2(char write_address, char read_address, int *values) {
int xMagH = (int)i2c2_read(write_address, read_address, MAG_XOUT_H);
int xMagL = (int)i2c2_read(write_address, read_address, MAG_XOUT_L);
int yMagH = (int)i2c2_read(write_address, read_address, MAG_YOUT_H);
int yMagL = (int)i2c2_read(write_address, read_address, MAG_YOUT_L);
int zMagH = (int)i2c2_read(write_address, read_address, MAG_ZOUT_H);
int zMagL = (int)i2c2_read(write_address, read_address, MAG_ZOUT_L);
values[0] = (xMagH << 8) + xMagL;
values[1] = (yMagH << 8) + yMagL;
values[2] = (zMagH << 8) + zMagL;
int xAccelH = (int)i2c2_read(write_address, read_address, MAG_ACCEL_XOUT_H);
int xAccelL = (int)i2c2_read(write_address, read_address, MAG_ACCEL_XOUT_L);
int yAccelH = (int)i2c2_read(write_address, read_address, MAG_ACCEL_YOUT_H);
int yAccelL = (int)i2c2_read(write_address, read_address, MAG_ACCEL_YOUT_L);
int zAccelH = (int)i2c2_read(write_address, read_address, MAG_ACCEL_ZOUT_H);
int zAccelL = (int)i2c2_read(write_address, read_address, MAG_ACCEL_ZOUT_L);
values[3] = (xAccelH << 8) + xAccelL;
values[4] = (yAccelH << 8) + yAccelL;
values[5] = (zAccelH << 8) + zAccelL;
}
/*
* File: TFT_test_BRL4.c
* Author: Bruce Land
* Adapted from:
* main.c by
* Author: Syed Tahmid Mahbub
* Target PIC: PIC32MX250F128B
*/
////////////////////////////////////
// clock AND protoThreads configure!
// You MUST check this file!
#include "config.h"
// threading library
#include "pt_cornell_1_2_1.h"
////////////////////////////////////
// graphics libraries
#include "tft_gfx.h"
#include "tft_master.h"
// need for rand function
#include <stdlib.h>
#include <math.h>
////////////////////////////////////
#include "i2c_helper.h"
// PORT B
#define EnablePullDownB(bits) CNPUBCLR=bits; CNPDBSET=bits;
#define DisablePullDownB(bits) CNPDBCLR=bits;
#define EnablePullUpB(bits) CNPDBCLR=bits; CNPUBSET=bits;
#define DisablePullUpB(bits) CNPUBCLR=bits;
//PORT A
#define EnablePullDownA(bits) CNPUACLR=bits; CNPDASET=bits;
#define DisablePullDownA(bits) CNPDACLR=bits;
#define EnablePullUpA(bits) CNPDACLR=bits; CNPUASET=bits;
#define DisablePullUpA(bits) CNPUACLR=bits;
// Wakeup register definitions
#define GYRO_STDBY_REG 0x43
#define GYRO_ODR_WAKE 0x41
#define GYRO_ACCEL_CTL 0x40
#define MAG_CNTL2 0x3A
char buffer[1000];
// === thread structures ============================================
// thread control structs
static struct pt pt_display, pt_DMA_output, pt_serial, pt_input;
// system 1 second interval tick
int sys_time_seconds;
#define VALUES_SIZE 6
static int shoulder_gyro[VALUES_SIZE];
static int elbow_gyro[VALUES_SIZE];
static int wrist_gyro[VALUES_SIZE];
static int shoulder_mag[VALUES_SIZE];
static int elbow_mag[VALUES_SIZE];
static int wrist_mag[VALUES_SIZE];
#define NUM_DEVICES 2
// {write, read}
static char gyro_addresses[NUM_DEVICES * 2] = {KIONIX_GYRO_W1, KIONIX_GYRO_R1,
KIONIX_GYRO_W2, KIONIX_GYRO_R2};
static char mag_addresses[NUM_DEVICES * 2] = {KIONIX_MAG_W1, KIONIX_MAG_R1,
KIONIX_MAG_W2, KIONIX_MAG_R2};
// === Thread 3 (ECE4760 UART) ======================================
static char cmd[16]; //pt_DMA_output
static double value;
float s, p, i, d;
static PT_THREAD (protothread_serial(struct pt *pt))
{
PT_BEGIN(pt);
while(1) {
read_gyro_values_shoulder(KIONIX_GYRO_WRITE, KIONIX_GYRO_READ, shoulder_gyro);
read_gyro_values_elbow(KIONIX_GYRO_WRITE, KIONIX_GYRO_READ, elbow_gyro);
read_gyro_values_wrist(KIONIX_GYRO_WRITE, KIONIX_GYRO_READ, wrist_gyro);
read_mag_values_shoulder(KIONIX_MAG_WRITE, KIONIX_MAG_READ, shoulder_mag);
read_mag_values_elbow(KIONIX_MAG_WRITE, KIONIX_MAG_READ, elbow_mag);
read_mag_values_wrist(KIONIX_MAG_WRITE, KIONIX_MAG_READ, wrist_mag);
// separator line
sprintf(PT_send_buffer, "init\r\n");
PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
// send the prompt
sprintf(PT_send_buffer, "%d %d %d %d %d %d\r\n", shoulder_gyro[0],
shoulder_gyro[1], shoulder_gyro[2], shoulder_gyro[3],
shoulder_gyro[4], shoulder_gyro[5]);
PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
sprintf(PT_send_buffer, "%d %d %d %d %d %d\r\n", elbow_gyro[0],
elbow_gyro[1], elbow_gyro[2], elbow_gyro[3], elbow_gyro[4],
elbow_gyro[5]);
PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
sprintf(PT_send_buffer, "%d %d %d %d %d %d\r\n", wrist_gyro[0],
wrist_gyro[1], wrist_gyro[2], wrist_gyro[3], wrist_gyro[4],
wrist_gyro[5]);
PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
sprintf(PT_send_buffer, "%d %d %d %d %d %d\r\n", shoulder_mag[1],
shoulder_mag[2], shoulder_mag[3], shoulder_mag[4], shoulder_mag[5]);
PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
sprintf(PT_send_buffer, "%d %d %d %d %d %d\r\n", elbow_mag[0], elbow_mag[1],
elbow_mag[2], elbow_mag[3], elbow_mag[4], elbow_mag[5]);
PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
sprintf(PT_send_buffer, "%d %d %d %d %d %d\r\n", wrist_mag[0], wrist_mag[1],
wrist_mag[2], wrist_mag[3], wrist_mag[4], wrist_mag[5]);
PT_SPAWN(pt, &pt_DMA_output, PT_DMA_PutSerialBuffer(&pt_DMA_output));
PT_YIELD_TIME_msec(1);
// never exit while
} // END WHILE(1)
PT_END(pt);
} // thread 3
// === Main ======================================================
void main(void) {
//Enable channel
//BRG computed value for the baud rate generator. The value is
// calculated as follows: BRG = (Fpb / 2 / baudrate) - 2.
PT_setup();
INTEnableSystemMultiVectoredInt();
// initialize i2c
// PBCLK 40Mhz, FSCK 400kHz -> 0x02C I2CxBRG
// src: table 24-2
// https://people.ece.cornell.edu/land/courses/ece4760/PIC32/index_i2c.html
OpenI2C1(I2C_ON, 0x02C);
//OpenI2C2(I2C_ON, 0x02C);
// bug in chip for when we use I2C1 and pin 3, line below from errata fixes
// thx tahmid
I2C1CONbits.DISSLW=1;
//I2C2CONbits.DISSLW=1;
// === config threads ==========
// turns OFF UART support and debugger pin, unless defines are set
//PT_setup();
// init the threads
PT_INIT(&pt_display);
PT_INIT(&pt_serial);
// init the display
tft_init_hw();
tft_begin();
tft_fillScreen(ILI9340_BLACK);
// 240x320 vertical display
tft_setRotation(0); // Use tft_setRotation(1) for 320x240
// moved here so we don't get interrupts before the display is ready
// === setup system wide interrupts ========
//INTEnableSystemMultiVectoredInt();
// seed random color
srand(1);
// Set up i2c pin control indexing
mPORTASetPinsDigitalOut(BIT_2 | BIT_3 | BIT_4);
// Set them to low
mPORTAClearBits(BIT_2 | BIT_3 | BIT_4);
/*
//XXX remove this; for testing
mPORTASetBits(BIT_2 | BIT_3 | BIT_4);
*/
// ========== Gyro Wakeup ============
// Values to write
// GYRO_STDBY_REG
char gyro_wakeup[1] = {0x0};
// GYRO_ODR_WAKE
char gyro_range_setup[1] = {0xF9};
// GYRO_ACCEL_CTL
char gyro_accel_setup[1];
// Wake up gyros
i2c_write_shoulder(KIONIX_GYRO_WRITE, GYRO_STDBY_REG, gyro_wakeup, 1);
i2c_write_elbow(KIONIX_GYRO_WRITE, GYRO_STDBY_REG, gyro_wakeup, 1);
i2c_write_wrist(KIONIX_GYRO_WRITE, GYRO_STDBY_REG, gyro_wakeup, 1);
// Apply range settings
i2c_write_shoulder(KIONIX_GYRO_WRITE, GYRO_ODR_WAKE, gyro_range_setup, 1);
i2c_write_elbow(KIONIX_GYRO_WRITE, GYRO_ODR_WAKE, gyro_range_setup, 1);
i2c_write_wrist(KIONIX_GYRO_WRITE, GYRO_ODR_WAKE, gyro_range_setup, 1);
// Apply acceleration settings
gyro_accel_setup[0] =
i2c_read_shoulder(KIONIX_GYRO_WRITE, KIONIX_GYRO_READ, GYRO_ACCEL_CTL);
gyro_accel_setup[0] |= 0xF3;
i2c_write_shoulder(KIONIX_GYRO_WRITE, GYRO_ACCEL_CTL, gyro_accel_setup, 1);
gyro_accel_setup[0] =
i2c_read_elbow(KIONIX_GYRO_WRITE, KIONIX_GYRO_READ, GYRO_ACCEL_CTL);
gyro_accel_setup[0] |= 0xF3;
i2c_write_elbow(KIONIX_GYRO_WRITE, GYRO_ACCEL_CTL, gyro_accel_setup, 1);
gyro_accel_setup[0] =
i2c_read_wrist(KIONIX_GYRO_WRITE, KIONIX_GYRO_READ, GYRO_ACCEL_CTL);
gyro_accel_setup[0] |= 0xF3;
i2c_write_wrist(KIONIX_GYRO_WRITE, GYRO_ACCEL_CTL, gyro_accel_setup, 1);
// ============ Mag Wakeup =========
// Wake up mags and apply range settings
char mag_wakeup[1] = {0x0};
mag_wakeup[0] = i2c_read_shoulder(KIONIX_MAG_WRITE, KIONIX_MAG_READ, MAG_CNTL2);
mag_wakeup[0] |= 0x8F;
i2c_write_shoulder(KIONIX_MAG_WRITE, MAG_CNTL2, mag_wakeup, 1);
mag_wakeup[0] = i2c_read_elbow(KIONIX_MAG_WRITE, KIONIX_MAG_READ, MAG_CNTL2);
mag_wakeup[0] |= 0x8F;
i2c_write_elbow(KIONIX_MAG_WRITE, MAG_CNTL2, mag_wakeup, 1);
mag_wakeup[0] = i2c_read_wrist(KIONIX_MAG_WRITE, KIONIX_MAG_READ, MAG_CNTL2);
mag_wakeup[0] |= 0x8F;
i2c_write_wrist(KIONIX_MAG_WRITE, MAG_CNTL2, mag_wakeup, 1);
// round-robin scheduler for threads
while (1) {
//PT_SCHEDULE(protothread_display(&pt_display));
PT_SCHEDULE(protothread_serial(&pt_serial));
}
} // main
// === end ======================================================
import serial
import io
import time
import numpy as np
from panda3d.core import LQuaternionf, LVecBase4f, VBase3
import copy
from pykalman import AdditiveUnscentedKalmanFilter, KalmanFilter
class Position:
def __init__(self, x=0, y=0, z=0, ax=None, ay=None, az=None):
self.x = x
self.y = y
self.z = z
self.ax = ax
self.ay = ay
self.az = az
def scale(self, value):
"""
Returns a scaled position.
"""
return Position(self.x * value, self.y * value, self.z * value)
def get_parent_accel(self):
return [self.ax, self.ay, self.az]
def __str__(self):
return "Position: %f %f %f" % (self.x, self.y, self.z)
class Velocity:
def __init__(self, dx=0, dy=0, dz=0):
self.dx = dx
self.dy = dy
self.dz = dz
def __str__(self):
return "Velocity: %f %f %f" % (self.dx, self.dy, self.dz)
class Orientation:
def __init__(self, x=0, y=0, z=0):
"""
Takes in an Euler angle and stores it as
a quaternion.
"""
q = LQuaternionf()
q.setHpr(VBase3(x, y, z))
self.q = [i for i in q]
self.x = x
self.y = y
self.z = z
def get_quaternion(self):
return LQuaternionf(LVecBase4f(self.q[0], self.q[1], self.q[2], self.q[3]))
def set_quaternion(self, h, p, r):
q = LQuaternionf()
q.setHpr(VBase3(h, p, r))
self.q = [i for i in q]
def get_xyz(self):
return [self.x, self.y, self.z]
def add(self, q2):
q1 = self.get_quaternion()
q1 = q1 * q2
q1.normalize()
self.q = [i for i in q1]
def __str__(self):
arr = [i for i in self.get_quaternion().getHpr()]
arr /= np.linalg.norm(arr)
arr = [round(k, 2) for k in arr]
return "%+.2f, %+.2f, %+.2f" % (arr[0], arr[1], arr[2])
class Pose:
def __init__(self, position=None, orientation=None):
if position is None:
self.position = Position()
else:
self.position = position
if orientation is None:
self.orientation = Orientation()
else:
self.orientation = orientation
def __str__(self):
p = str(self.position)
o = str(self.orientation)
return "%s\n%s" % (p, o)
class AccelerometerData:
def __init__(self, values, calib=[0,0,0], gyro=None):
calib = [i for i in calib]
if gyro is not None:
calib = gyro.xform(VBase3(calib[0], calib[1], calib[2]))
self.ax = values[3] - calib[0]
self.ay = values[4] - calib[1]
self.az = values[5] - calib[2]
def integrate(self, pose, velocity, dt):
# Passing along accel information to the position
pose.position.ax = self.ax
pose.position.ay = self.ay
pose.position.az = self.az
# Normal double integration
pose.position.x += velocity.dx * dt
pose.position.y += velocity.dy * dt
pose.position.z += velocity.dz * dt
velocity.dx += self.ax * dt
velocity.dy += self.ay * dt
velocity.dz += self.az * dt
class MagnetometerData:
def __init__(self, values, calib=[0,0,0]):
self.x = values[0] - calib[0]
self.y = values[1] - calib[0]
self.z = values[2] - calib[0]
def compute_heading(self):
pass
#self.MagnetometerData
class GyroscopeData:
def __init__(self, values, calib=[0,0,0]):
"""
Units are degrees per second.
"""
thresh = 700
self.dx = values[0] - calib[0]
self.dy = values[1] - calib[1]
self.dz = values[2] - calib[2]
self.original = values[:3]
def integrate(self, pose, dt, accel=None):
angular_change = LQuaternionf()
dt = 0.002 # calibration
if accel is None:
angular_change.setHpr(VBase3(self.dx * dt, self.dy * dt, self.dz * dt))
pose.orientation.add(angular_change)
else:
gyrData_dt = VBase3(self.dx * dt, self.dy * dt, self.dz * dt)
old_angle = pose.orientation.get_quaternion().getHpr()
accels = [accel.ax, accel.ay, accel.az]
# Avoid divide by zero errors
if abs(np.linalg.norm(accels)) > 2**(-25):
accels = [i / np.linalg.norm(accels) for i in accels]
angle = [0, 0, 0]
# Complementary filter is implemented here!
for i in range(3):
angle[i] = 0.97 * (old_angle[i] + gyrData_dt[i]) + 0.03 * accels[i]
if np.linalg.norm(gyrData_dt) < 1: angle[i] = old_angle[i]
pose.orientation.set_quaternion(angle[0], angle[1], angle[2])
# don't touch this it's needed to propagate orientations
pose.orientation.x = self.original[0]
pose.orientation.y = self.original[1]
pose.orientation.z = self.original[2]
class Odometry:
def __init__(self, odometry):
if type(odometry) != dict:
raise TypeError("Odometry should be a dictionary.")
self.odometry = odometry
def orientation_select(self, old_quat, new_quat, old_mag, new_mag):
# High pass filter
"""
# note: pass around angular change value if this doesn't work
inverted = LQuaternionf()
inverted.invertFrom(old_quat)
rotation = new_quat * inverted
# if the threshold is below some value, we use magnetometer
stationary_or_drifting_threshold = 0.1
print "Magnitude: ", magnitude
if magnitude < stationary_or_drifting_threshold:
return mag_quat
else:
return new_quat
"""
return new_quat
def get_wrist_orientation(self, prev):
old_quat = prev.odometry["wrist_gyro"][0].orientation.get_quaternion()
new_quat = self.odometry["wrist_gyro"][0].orientation.get_quaternion()
old_mag = prev.odometry["wrist_mag"][0].orientation.get_xyz()
new_mag = self.odometry["wrist_mag"][0].orientation.get_xyz()
return self.orientation_select(old_quat, new_quat, old_mag, new_mag)
def get_wrist_pos(self):
return self.odometry["wrist_gyro"][0].position
def create_odometry():
return {
"wrist_mag": [Pose(), Velocity()],
"elbow_mag": [Pose(), Velocity()],
"wrist_gyro": [Pose(), Velocity()],
"elbow_gyro": [Pose(), Velocity()],
"shoulder_mag": [Pose(), Velocity()],
"shoulder_gyro": [Pose(), Velocity()]
}
def wait_until_transmission_start(ser):
while True:
# Used to indicate start of transmission
line = ser.readline().strip()
if "init" in line:
break
else:
print "ERROR: received non init line", line
debug = None
def compute_special_calib(odometries, key):
"""
Calibrates magnetometers and gyros (first 3 values)
"""
s = [0, 0, 0]
darstring = ""
for odom in odometries:
xyz = odom[key][0].orientation.get_xyz()
for i in range(3):
s[i] += xyz[i]
darstring += "%d %d %d\n" % (xyz[0], xyz[1], xyz[2])
# optionally write data to file for visualization
'''
if key == "wrist_mag":
with open("dataz", "w") as f:
f.write(darstring)
'''
return [i / len(odometries) for i in s]
def compute_accel_calib_median(odometries, key):
"""
Calibrates accelerometers by median value.
"""
s = [0, 0, 0]
odometries.sort(key=lambda x: np.linalg.norm(x[key][0].position.get_parent_accel()))
return odometries[len(odometries)/2][key][0].position.get_parent_accel()
def compute_accel_calib_avg(odometries, key):
"""
Calibrates accelerometers by average value.
"""
s = [0, 0, 0]
for odometry in odometries:
xyz = odometry[key][0].position.get_parent_accel()
for i in range(3):
s[i] += xyz[i]
return [i / len(odometries) for i in s]
"""
Kalman filter default functions.
"""
# initialize parameters
def transition_function(state, noise):
a = state[0] * np.sin(state[1]) + noise[0]
b = state[1] + noise[1]
return np.array([a, b])
def observation_function(state, noise):
C = np.array([[-1, 0.5], [0.2, 0.1]])
return np.dot(C, state) + noise
def additive_transition_function(state):
return transition_function(state, np.array([0, 0]))
def additive_observation_function(state):
return observation_function(state, np.array([0, 0]))
transition_covariance = np.eye(2)
random_state = np.random.RandomState(0)
observation_covariance = np.eye(2) + random_state.randn(2, 2) * 0.1
initial_state_mean = [0, 0]
initial_state_covariance = [[1, 0.1], [ 0.1, 1]]
def worker(queue=None):
"""
Main reader thread. Communicates with game through queue.
Communicates with PIC32 over tty.usbserial.
"""
odometry = create_odometry()
t1 = time.time()
elbow_gyro = None
wrist_gyro = None
shoulder_gyro = None
elbow_mag = None
wrist_mag = None
shoulder_mag = None
wrist_mag_calib = [0, 0, 0]
wrist_gyro_calib = [0, 0, 0]
elbow_mag_calib = [0, 0, 0]
elbow_gyro_calib = [0, 0, 0]
shoulder_mag_calib = [0, 0, 0]
shoulder_gyro_calib = [0, 0, 0]
wrist_mag_accel_calib = [0, 0, 0]
wrist_gyro_accel_calib = [0, 0, 0]
elbow_mag_accel_calib = [0, 0 ,0]
elbow_gyro_accel_calib = [0, 0, 0]
shoulder_mag_accel_calib = [0, 0, 0]
shoulder_gyro_accel_calib = [0, 0, 0]
avg_wrist = []
# Used to compute calibration data
odometries = []
# Currently we read at about 41Hz
dt = 1 / float(41)
#ukf = AdditiveUnscentedKalmanFilter(lambda x: x, lambda x, v: x + v)
#ukf = KalmanFilter(lambda x: x, lambda y: y)
#ukf = AdditiveUnscentedKalmanFilter(
# additive_transition_function, additive_observation_function,
# transition_covariance, observation_covariance,
# initial_state_mean, initial_state_covariance
#)
#means = []
#covariances = []
#mean, covariance = ukf.filter([1, 1])
#means.append(mean)
#covariances.append(covariance)
print "attempting to acquire serial conn"
with serial.Serial('/dev/tty.usbserial', baudrate=115200, timeout=1) as ser:
i = 0
readings = []
while True:
wait_until_transmission_start(ser)
s = ""
for j in range(6):
line = ser.readline().strip().lstrip("\x00").lstrip("C!").lstrip('\xf8')
line = line.rstrip("\x00")
s += line + "\n"
lines = s.split("\n")[:-1]
for device_select, line in enumerate(lines):
try:
line = [int(j) for j in line.split(" ")]
except:
print "ERROR: failed to parse line ", line
if device_select == 0:
shoulder_gyro = line
accel_data = AccelerometerData(line, shoulder_gyro_accel_calib)
accel_data.integrate(pose=odometry["shoulder_gyro"][0],
velocity=odometry["shoulder_gyro"][1], dt=dt)
gyro_data = GyroscopeData(line, shoulder_gyro_calib)
x = gyro_data.dx
y = gyro_data.dy
z = gyro_data.dz
gyro_data.dx = -z
gyro_data.dy = -y
gyro_data.dz = x
gyro_data.integrate(pose=odometry["shoulder_gyro"][0], dt=dt,
accel=accel_data)
elif device_select == 1:
elbow_gyro = line
accel_data = AccelerometerData(line, elbow_gyro_accel_calib)
accel_data.integrate(pose=odometry["elbow_gyro"][0],
velocity=odometry["elbow_gyro"][1], dt=dt)
gyro_data = GyroscopeData(line, elbow_gyro_calib)
x = gyro_data.dx
y = gyro_data.dy
z = gyro_data.dz
gyro_data.dx = z
gyro_data.dy = -y
gyro_data.dz = -x
gyro_data.integrate(pose=odometry["elbow_gyro"][0], dt=dt,
accel=accel_data)
elif device_select == 2:
wrist_gyro = line
accel_data = AccelerometerData(line, wrist_gyro_accel_calib,
odometry["wrist_gyro"][0].orientation.get_quaternion())
#accel_data.integrate(pose=odometry["wrist_gyro"][0],
# velocity=odometry["wrist_gyro"][1], dt=dt)
#measurement = [accel_data.ax, accel_data.ay, accel_data.az]
#measurement = measurement[:2]
#print "filtered: ", means[-1]
#print "filtered cov: ", covariances[-1]
#next_mean, next_covariance = ukf.filter_update(means[-1], covariances[-1], measurement)
#means.append(next_mean)
#next_covariance.append(covariance)
#flat_filtered = np.ndarray.flatten(means[-1])
#print "last mean: ", means
#accel_data = AccelerometerData(means[-1] + [0, 0, 0, 0])
accel_data.integrate(pose=odometry["wrist_gyro"][0],
velocity=odometry["wrist_gyro"][1], dt=dt)
gyro_data = GyroscopeData(line, wrist_gyro_calib)
x = gyro_data.dx
y = gyro_data.dy
z = gyro_data.dz
gyro_data.dx = z
gyro_data.dy = -y
gyro_data.dz = x
gyro_data.integrate(pose=odometry["wrist_gyro"][0], dt=dt, accel=accel_data)
elif device_select == 3:
shoulder_mag = line
accel_data = AccelerometerData(line, shoulder_mag_accel_calib)
accel_data.integrate(pose=odometry["shoulder_mag"][0],
velocity=odometry["shoulder_mag"][1], dt=dt)
mag_data = MagnetometerData(line, shoulder_mag_calib)
orientation = Orientation(mag_data.x, mag_data.y, mag_data.z)
odometry["shoulder_mag"][0].orientation = orientation
elif device_select == 4:
elbow_mag = line
accel_data = AccelerometerData(line, shoulder_gyro_accel_calib)
accel_data.integrate(pose=odometry["elbow_mag"][0],
velocity=odometry["elbow_mag"][1], dt=dt)
mag_data = MagnetometerData(line, elbow_mag_calib)
orientation = Orientation(mag_data.x, mag_data.y, mag_data.z)
odometry["elbow_mag"][0].orientation = orientation
else:
wrist_mag = line
accel_data = AccelerometerData(line, wrist_mag_accel_calib)
accel_data.integrate(pose=odometry["wrist_mag"][0],
velocity=odometry["wrist_mag"][1], dt=dt)
mag_data = MagnetometerData(line, wrist_mag_calib)
odometry["wrist_mag"][0].orientation = orientation
if queue is not None:
calibration_period = 120
if len(odometries) < calibration_period:
odometries.append(copy.deepcopy(odometry))
elif len(odometries) == calibration_period:
wrist_gyro_calib = compute_special_calib(odometries, "wrist_gyro")
wrist_gyro_accel_calib = compute_accel_calib_avg(odometries, "wrist_gyro")
#elbow_gyro_calib = compute_special_calib(odometries, "elbow_gyro")
#elbow_gyro_accel_calib = compute_accel_calib(odometries, "elbow_gyro")
#shoulder_gyro_calib = compute_special_calib(odometries, "shoulder_gyro")
#shoulder_gyro_accel_calib = compute_accel_calib(odometries, "shoulder_gyro")
#wrist_mag_accel_calib = compute_accel_calib(odometries, "wrist_mag")
wrist_mag_calib = compute_special_calib(odometries, "wrist_mag")
print "Calibrated all sensors to average/median values in last 3 seconds."
#print "Acceleration: ", wrist_gyro_accel_calib
#print "line: ", wrist_gyro
odometries.append(odometry)
odometry = create_odometry()
else:
queue.put(Odometry(odometry))
i += 1
if time.time() - t1 > 1.0:
if queue is None:
print "rate (Hz): ", i
print "shoulder gyro: ", shoulder_gyro
print "shoulder mag: ", shoulder_mag
print "elbow gyro: ", elbow_gyro
print "elbow mag: ", elbow_mag
print "wrist gyro: ", wrist_gyro
print "wrist mag: ", wrist_mag
#for item in odometry:
# print "odometry for sensor %s:" % item
# print odometry[item][0]
# print odometry[item][1
#print odometry["wrist_gyro"][0].orientation
#print "%d %d %d" % (debug.dx, debug.dy, debug.dz)
t1 = time.time()
i = 0
if __name__ == "__main__":
worker()
import threading
import time
import sys
import multiprocessing
from game import *
from read_sensors import worker
def feeder_thread(queue, hungry_child):
while True:
odometry = queue.get()
hungry_child.feedValue(odometry)
if __name__ == '__main__':
queue = multiprocessing.Queue()
p = multiprocessing.Process(target=worker, args=(queue,))
# Daemonize reader process so that when this program quits,
# the process is killed too
p.daemon = True
p.start()
game = Game()
thread = threading.Thread(target = feeder_thread, args=(queue, game))
# Daemonize game feeder thread so that when this program quits,
# the thread is killed too
thread.daemon = True
thread.start()
run()
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
# Code to plot 3D points in a scatter plot
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
c = 'r'
m = 'o'
data = [[int(j) for j in i.strip().split(" ")] for i in open("dataz").readlines()]
for item in data:
ax.scatter(item[0], item[1], item[2], c=c, marker=m)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
import sys
import direct.directbase.DirectStart
from direct.showbase.DirectObject import DirectObject
from direct.showbase.InputStateGlobal import inputState
from panda3d.core import AmbientLight
from panda3d.core import DirectionalLight
from panda3d.core import Vec3
from panda3d.core import Vec4
from panda3d.core import Quat
from panda3d.core import Point3
from panda3d.core import TransformState
from panda3d.core import BitMask32
from panda3d.core import LQuaternionf, LVecBase4f, VBase3
from panda3d.physics import ForceNode, LinearVectorForce
from panda3d.bullet import BulletWorld
from panda3d.bullet import BulletBoxShape
from panda3d.bullet import BulletCylinderShape
from panda3d.bullet import BulletSphereShape
from panda3d.bullet import BulletRigidBodyNode
from panda3d.bullet import BulletGenericConstraint
from panda3d.bullet import BulletSphericalConstraint
from panda3d.bullet import BulletSliderConstraint
from panda3d.bullet import BulletDebugNode
import logging
import threading
import time
import multiprocessing
from read_sensors import worker
import numpy as np
class Game(DirectObject):
def __init__(self):
self.odometry = None
self.prev_odometry = None
self.total_time = 0
# game objects to be manipulated by sensor data
self.wrist = None
self.elbow = None
self.shoulder = None
self.upper_arm = None
self.forearm = None
base.setBackgroundColor(0.1, 0.1, 0.8, 1)
base.setFrameRateMeter(True)
base.cam.setPos(0, -20, 5)
base.cam.lookAt(0, 0, 0)
# Light
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
dlight = DirectionalLight('directionalLight')
dlight.setDirection(Vec3(1, 1, -1))
dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
dlightNP = render.attachNewNode(dlight)
render.clearLight()
render.setLight(alightNP)
render.setLight(dlightNP)
# Input
self.accept('escape', self.doExit)
self.accept('r', self.doReset)
self.accept('f1', self.toggleWireframe)
self.accept('f2', self.toggleTexture)
self.accept('f3', self.toggleDebug)
self.accept('f5', self.doScreenshot)
# Task
taskMgr.add(self.update, 'updateWorld')
# Physics
self.setup()
# _____HANDLER_____
def doExit(self):
self.cleanup()
sys.exit(1)
def doReset(self):
self.cleanup()
self.setup()
def toggleWireframe(self):
base.toggleWireframe()
def toggleTexture(self):
base.toggleTexture()
def toggleDebug(self):
if self.debugNP.isHidden():
self.debugNP.show()
else:
self.debugNP.hide()
def doScreenshot(self):
base.screenshot('Bullet')
def doRemove(self, bodyNP, task):
self.world.removeRigidBody(bodyNP.node())
bodyNP.removeNode()
return task.done
# ____TASK___
def update(self, task):
dt = globalClock.getDt()
self.world.doPhysics(dt, 20, 1.0/180.0)
self.total_time += dt
if all([e is not None for e in [self.odometry, self.shoulder, self.prev_odometry]]):
# recieve sensor data
upper_arm_quat = self.odometry.odometry["shoulder_gyro"][0].orientation.get_quaternion()
forearm_quat = self.odometry.odometry["elbow_gyro"][0].orientation.get_quaternion()
wrist_quat = self.odometry.odometry["wrist_gyro"][0].orientation.get_quaternion()
# forward kinematics
self.shoulder.setQuat(upper_arm_quat)
upper_arm_length = 4
upper_arm_pos = upper_arm_quat.getRight() * -upper_arm_length/2
self.upper_arm.setQuat(upper_arm_quat)
self.upper_arm.setPos(upper_arm_pos)
elbow_pos = upper_arm_quat.getRight() * -upper_arm_length
self.elbow.setQuat(forearm_quat)
self.elbow.setPos(elbow_pos)
self.forearm.setQuat(forearm_quat)
forearm_length = 4
forearm_pos = elbow_pos + forearm_quat.getRight() * -forearm_length/2
self.forearm.setPos(forearm_pos)
self.wrist.setQuat(wrist_quat)
wrist_pos = elbow_pos + forearm_quat.getRight() * -forearm_length
self.wrist.setPos(wrist_pos)
return task.cont
def cleanup(self):
self.worldNP.removeNode()
self.worldNP = None
self.world = None
def feedValue(self, new_odometry):
self.prev_odometry = self.odometry
self.odometry = new_odometry
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.debugNP.node().showWireframe(True)
self.debugNP.node().showConstraints(False)
self.debugNP.node().showBoundingBoxes(False)
self.debugNP.node().showNormals(False)
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# floor
floor_shape = BulletBoxShape(Vec3(10, 10, 0.1))
floor_body = BulletRigidBodyNode('Floor Body')
floor_bodyNP = self.worldNP.attachNewNode(floor_body)
floor_bodyNP.node().addShape(floor_shape)
floor_bodyNP.setCollideMask(BitMask32.allOn())
floor_bodyNP.setPos(-4, 0, -4)
floor_bodyNP.setHpr(Vec3(0,0,-1))
self.world.attachRigidBody(floor_body)
# ceiling
ceiling_shape = BulletBoxShape(Vec3(10, 10, 0.1))
ceiling_body = BulletRigidBodyNode('Ceiling Body')
ceiling_bodyNP = self.worldNP.attachNewNode(ceiling_body)
ceiling_bodyNP.node().addShape(ceiling_shape)
ceiling_bodyNP.setCollideMask(BitMask32.allOn())
ceiling_bodyNP.setPos(-4, 0, 5)
self.world.attachRigidBody(ceiling_body)
# walls
# back wall
wall_shape = BulletBoxShape(Vec3(10, 0.1, 10))
wall_body = BulletRigidBodyNode('Wall1 Body')
wall_bodyNP = self.worldNP.attachNewNode(wall_body)
wall_bodyNP.node().addShape(wall_shape)
wall_bodyNP.setCollideMask(BitMask32.allOn())
wall_bodyNP.setPos(-4, 4, 4)
self.world.attachRigidBody(wall_body)
# right wall
wall_shape = BulletBoxShape(Vec3(0.1, 10, 10))
wall_body = BulletRigidBodyNode('Wall2 Body')
wall_bodyNP = self.worldNP.attachNewNode(wall_body)
wall_bodyNP.node().addShape(wall_shape)
wall_bodyNP.setCollideMask(BitMask32.allOn())
wall_bodyNP.setPos(0, 0, 4)
self.world.attachRigidBody(wall_body)
# left wall
wall_shape = BulletBoxShape(Vec3(0.1, 10, 10))
wall_body = BulletRigidBodyNode('Wall3 Body')
wall_bodyNP = self.worldNP.attachNewNode(wall_body)
wall_bodyNP.node().addShape(wall_shape)
wall_bodyNP.setCollideMask(BitMask32.allOn())
wall_bodyNP.setPos(-8, 0, 4)
self.world.attachRigidBody(wall_body)
# front wall
wall_shape = BulletBoxShape(Vec3(10, 0.1, 10))
wall_body = BulletRigidBodyNode('Wall4 Body')
wall_bodyNP = self.worldNP.attachNewNode(wall_body)
wall_bodyNP.node().addShape(wall_shape)
wall_bodyNP.setCollideMask(BitMask32.allOn())
wall_bodyNP.setPos(-4, -4, 4)
self.world.attachRigidBody(wall_body)
# balls
for i in xrange(15):
ball_shape = BulletSphereShape(0.6)
ball_body = BulletRigidBodyNode('Ball Body')
ball_bodyNP = self.worldNP.attachNewNode(ball_body)
ball_bodyNP.node().addShape(ball_shape)
ball_bodyNP.node().setMass(1.0)
ball_bodyNP.setCollideMask(BitMask32.allOn())
ball_bodyNP.setPos(-2, 0, 0)
self.world.attachRigidBody(ball_body)
# wrist
wrist_shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
wrist_body = BulletRigidBodyNode('Wrist Body')
wrist_bodyNP = self.worldNP.attachNewNode(wrist_body)
wrist_bodyNP.node().addShape(wrist_shape)
wrist_bodyNP.node().setKinematic(True)
wrist_bodyNP.setCollideMask(BitMask32.allOn())
wrist_bodyNP.setPos(-8, 0, 0)
self.world.attachRigidBody(wrist_body)
self.wrist = wrist_bodyNP
self.wrist_phys = wrist_body
# elbow
elbow_shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
elbow_body = BulletRigidBodyNode('Elbow Body')
elbow_bodyNP = self.worldNP.attachNewNode(elbow_body)
elbow_bodyNP.node().addShape(elbow_shape)
elbow_bodyNP.node().setKinematic(True)
elbow_bodyNP.setCollideMask(BitMask32.allOn())
elbow_bodyNP.setPos(-4, 0, 0)
self.world.attachRigidBody(elbow_body)
self.elbow = elbow_bodyNP
# shoulder
shoulder_shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
shoulder_body = BulletRigidBodyNode('Shoulder Body')
shoulder_bodyNP = self.worldNP.attachNewNode(shoulder_body)
shoulder_bodyNP.node().addShape(shoulder_shape)
shoulder_bodyNP.node().setKinematic(True)
shoulder_bodyNP.setCollideMask(BitMask32.allOn())
shoulder_bodyNP.setPos(0, 0, 0)
self.world.attachRigidBody(shoulder_body)
self.shoulder = shoulder_bodyNP
# forearm
forearm_shape = BulletCylinderShape(0.2, 2, 0)
forearm_body = BulletRigidBodyNode('Forearm Body')
forearm_bodyNP = self.worldNP.attachNewNode(forearm_body)
forearm_bodyNP.node().addShape(forearm_shape)
forearm_bodyNP.setCollideMask(BitMask32.allOn())
forearm_bodyNP.setPos(-6, 0, 0)
self.world.attachRigidBody(forearm_body)
self.forearm = forearm_bodyNP
# upper arm
upper_arm_shape = BulletCylinderShape(0.2, 2, 0)
upper_arm_body = BulletRigidBodyNode('Upper Arm Body')
upper_arm_bodyNP = self.worldNP.attachNewNode(upper_arm_body)
upper_arm_bodyNP.node().addShape(upper_arm_shape)
upper_arm_bodyNP.setCollideMask(BitMask32.allOn())
upper_arm_bodyNP.setPos(-2, 0, 0)
self.world.attachRigidBody(upper_arm_body)
self.upper_arm = upper_arm_bodyNP
if __name__ == "__main__":
game = Game()
run()
Daryl focused on the software.
Emma focused on the hardware.
Zachary worked on software and hardware.
*the schematic shows KMX61 eval board but we actually use KMX62 with the same eval board connections
