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

Table of Contents

Table of Contents

Introduction

Sound Byte

Why

What

High Level Design

Rationale

IEEE Standards

Background Information

Accelerometer

Gyroscope

Magnetometer

Forward Kinematics

Similar Designs

Hardware/Software Tradeoffs

Graphics Capabilities

Sensor Indexing

Software Design

Details

config.h

i2c_helper.h

pose.c

read_sensors.py

controller.py

plot.py

game.py

Design Resources

Discarded Designs

Hardware Design

Details

Constructing Proper Voltages

Design Resources

Discarded Designs

Results

Calibration Data

Safety

Execution Speed

Accuracy

Demonstration Video

Conclusion

Expectations

Intellectual Property

Ethical Considerations

Legal Considerations

Permissions

Research

Software Licensing

Appendix

A: Cornell ECE4760 Usage

B: Commented Program Listing

config.h

i2c_helper.h

pose.c

read_sensors.py

controller.py

plot.py

game.py

C: Overall Schematic

D: Cost Details

E: Work Breakdown

Daryl

Emma

Zachary

References and Acknowledgements

From kionix.com

KMX62 - mag-accel sensor unit

KXG03 - gyro-accel sensor unit

Previous Project

Other

Introduction

Sound Byte

We created an arm mounted sensor array which interfaces with a PIC32 unit to produce odometry data for arm motion tracking.

Why

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.

What

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.

High Level Design

Rationale

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.

IEEE Standards

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

Background Information

Accelerometer

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.

Gyroscope

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.

Magnetometer

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.

Forward Kinematics

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.

Similar Designs

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.

Hardware/Software Tradeoffs

Graphics Capabilities

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.

Sensor Indexing

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.

Software Design

Details

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.

config.h

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.

i2c_helper.h

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.

pose.c

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

read_sensors.py

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.

controller.py

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.

plot.py

We used this to create a 3D scatter plot.

game.py

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.

Design Resources

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.

Discarded Designs

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.

Hardware Design

Details

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.

UPDATED pose sensor locations.png

Constructing Proper Voltages

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.

Design Resources

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.

Discarded Designs

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.

Results

Calibration Data

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.

Safety

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.

Execution Speed

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.

Accuracy

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.

Demonstration Video

Our demo from class shows the technology off pretty well, and can be seen here.

output.gif

Conclusion

Expectations

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.

Intellectual Property

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.

Ethical Considerations

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.

Legal Considerations

There are none we are aware of, as we used wired communication only.

Permissions

Research

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.

Software Licensing

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.

Appendix

A: Cornell ECE4760 Usage

The group approves this report for inclusion on the course website. The group approves the video for inclusion on the course YouTube channel.

B: Commented Program Listing

config.h

/*
 * 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 */

i2c_helper.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;
}

pose.c

/*
 * 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  ======================================================

read_sensors.py

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()

controller.py

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()

plot.py

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()

game.py

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()

C: Overall Schematic

D: Cost Details

E: Work Breakdown

Daryl

Daryl focused on the software.

 

Emma

Emma focused on the hardware.

Zachary

Zachary worked on software and hardware.

References and Acknowledgements

From kionix.com

KMX62 - mag-accel sensor unit

Datasheet

Eval Board Schematic

*the schematic shows KMX61 eval board but we actually use KMX62 with the same eval board connections

KXG03 - gyro-accel sensor unit

Datasheet

Eval Board Schematic

Previous Project

Balancing Robot 201

Other