lorenzoriano

#!/usr/bin/env python
import sys
rospkgs = sys.argv[1:]
import roslib

for rospkg in rospkgs:
    roslib.load_manifest(rospkg)

paths = [path for path in sys.path if not path.startswith('/usr')]
#create a set to remove repetitions

lorenzoriano

import numpy as n
import scipy.interpolate
import scipy.ndimage

def congrid(a, newdims, method='linear', centre=False, minusone=False):
    '''Arbitrary resampling of source array to new dimension sizes.
    Currently only supports maintaining the same number of dimensions.
    To use 1-D arrays, first promote them to shape (x,1).
    
    Uses the same parameters and creates the same co-ordinate lookup points

lorenzoriano

import inspect
import sys
import os


class DebugPrint(object):
    def __init__(self, f):
        self.f = f

    def write(self, text):

lorenzoriano

import numpy as np,numpy.linalg

def _getAplus(A):
    eigval, eigvec = np.linalg.eig(A)
    Q = np.matrix(eigvec)
    xdiag = np.matrix(np.diag(np.maximum(eigval, 0)))
    return Q*xdiag*Q.T

def _getPs(A, W=None):
    W05 = np.matrix(W**.5)

lorenzoriano

#ifndef TIMER_H
#define TIMER_H

#include <ctime>

//link with rt (-lrt)

class timer {
public:
    timer() {

lorenzoriano

template <typename T>
std::vector<T> linspace(T a, T b, size_t N) {
    T h = (b - a) / static_cast<T>(N-1);
    std::vector<T> xs(N);
    typename std::vector<T>::iterator x;
    T val;
    for (x = xs.begin(), val = a; x != xs.end(); ++x, val += h)
        *x = val;
    return xs;
}

lorenzoriano

static const double     _PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348;
static const double _TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696;

// Floating-point modulo
// The result (the remainder) has same sign as the divisor.
// Similar to matlab's mod(); Not similar to fmod() -   Mod(-3,4)= 1   fmod(-3,4)= -3
template<typename T>
T Mod(T x, T y)
{
    static_assert(!std::numeric_limits<T>::is_exact , "Mod: floating-point type expected");

lorenzoriano

#!/usr/bin/python
from numpy import *
from traitsui.key_bindings import KeyBinding,KeyBindings
from traitsui.api import *
from traits.api import *
import numpy as np
import roslib
roslib.load_manifest("sensor_msgs")
roslib.load_manifest("rospy")
roslib.load_manifest("tf")

lorenzoriano

import numpy as np
from math import pi, log
import pylab
from scipy import fft, ifft
from scipy.optimize import curve_fit

i = 10000
x = np.linspace(0, 3.5 * pi, i)
y = (0.3*np.sin(x) + np.sin(1.3 * x) + 0.9 * np.sin(4.2 * x) + 0.06 *
    np.random.randn(i))

lorenzoriano

def trapez_profile(t, accel, vel_max, v_start, v_end, t_total):
    """Trapezoidal velocity profile function, handles special cases too
    
    Parameters:
    t: current time
    accel: the maximum acceleration (and deceleration) of the system
    vel_max: the maximum velocity
    v_start: the starting velocity
    v_end: the desired ending velocity
    t_toal: how long should the motion be