def test_scalar(self):
P = PiecewisePolynomial(self.xi,self.yi,3)
assert_almost_equal(P(self.test_xs[0]),self.spline_ys[0])
assert_almost_equal(P.derivative(self.test_xs[0],1),self.spline_yps[0])
assert_almost_equal(P(np.array(self.test_xs[0])),self.spline_ys[0])
assert_almost_equal(P.derivative(np.array(self.test_xs[0]),1),
self.spline_yps[0])
def test_scalar(self):
P = PiecewisePolynomial(self.xi,self.yi,3)
assert_almost_equal(P(self.test_xs[0]),self.spline_ys[0])
assert_almost_equal(P.derivative(self.test_xs[0],1),self.spline_yps[0])
assert_almost_equal(P(np.array(self.test_xs[0])),self.spline_ys[0])
assert_almost_equal(P.derivative(np.array(self.test_xs[0]),1),
self.spline_yps[0])
def test_shapes_scalarvalue_derivative(self):
P = PiecewisePolynomial(self.xi,self.yi,4)
n = 4
assert_array_equal(np.shape(P.derivative(0,1)), ())
assert_array_equal(np.shape(P.derivative(np.array(0),1)), ())
assert_array_equal(np.shape(P.derivative([0],1)), (1,))
assert_array_equal(np.shape(P.derivative([0,1],1)), (2,))
def test_shapes_scalarvalue_derivative(self):
P = PiecewisePolynomial(self.xi, self.yi, 4)
n = 4
assert_array_equal(np.shape(P.derivative(0, 1)), ())
assert_array_equal(np.shape(P.derivative(np.array(0), 1)), ())
assert_array_equal(np.shape(P.derivative([0], 1)), (1, ))
assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, ))
def test_shapes_vectorvalue_derivative(self):
P = PiecewisePolynomial(self.xi,
np.multiply.outer(self.yi, np.arange(3)), 4)
n = 4
assert_array_equal(np.shape(P.derivative(0, 1)), (3, ))
assert_array_equal(np.shape(P.derivative([0], 1)), (1, 3))
assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, 3))
def test_scalar(self):
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=DeprecationWarning)
P = PiecewisePolynomial(self.xi, self.yi, 3)
assert_almost_equal(P(self.test_xs[0]), self.spline_ys[0])
assert_almost_equal(P.derivative(self.test_xs[0], 1), self.spline_yps[0])
assert_almost_equal(P(np.array(self.test_xs[0])), self.spline_ys[0])
assert_almost_equal(P.derivative(np.array(self.test_xs[0]), 1), self.spline_yps[0])
def test_shapes_vectorvalue_derivative(self):
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=DeprecationWarning)
P = PiecewisePolynomial(self.xi, np.multiply.outer(self.yi, np.arange(3)), 4)
n = 4
assert_array_equal(np.shape(P.derivative(0, 1)), (3,))
assert_array_equal(np.shape(P.derivative([0], 1)), (1, 3))
assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, 3))
def test_shapes_vectorvalue_derivative(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi, np.multiply.outer(self.yi,
np.arange(3)),4)
n = 4
assert_array_equal(np.shape(P.derivative(0,1)), (3,))
assert_array_equal(np.shape(P.derivative([0],1)), (1,3))
assert_array_equal(np.shape(P.derivative([0,1],1)), (2,3))
def test_shapes_scalarvalue_derivative(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi,self.yi,4)
n = 4
assert_array_equal(np.shape(P.derivative(0,1)), ())
assert_array_equal(np.shape(P.derivative(np.array(0),1)), ())
assert_array_equal(np.shape(P.derivative([0],1)), (1,))
assert_array_equal(np.shape(P.derivative([0,1],1)), (2,))
def test_shapes_scalarvalue_derivative(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi, self.yi, 4)
n = 4
assert_array_equal(np.shape(P.derivative(0, 1)), ())
assert_array_equal(np.shape(P.derivative(np.array(0), 1)), ())
assert_array_equal(np.shape(P.derivative([0], 1)), (1, ))
assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, ))
def test_scalar(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi,self.yi,3)
assert_almost_equal(P(self.test_xs[0]),self.spline_ys[0])
assert_almost_equal(P.derivative(self.test_xs[0],1),self.spline_yps[0])
assert_almost_equal(P(np.array(self.test_xs[0])),self.spline_ys[0])
assert_almost_equal(P.derivative(np.array(self.test_xs[0]),1),
self.spline_yps[0])
def test_derivatives(self):
P = PiecewisePolynomial(self.xi, self.yi, 3)
m = 4
r = P.derivatives(self.test_xs, m)
#print r.shape, r
for i in xrange(m):
assert_almost_equal(P.derivative(self.test_xs, i), r[i])
def test_derivatives(self):
P = PiecewisePolynomial(self.xi, self.yi, 3)
m = 4
r = P.derivatives(self.test_xs, m)
# print r.shape, r
for i in xrange(m):
assert_almost_equal(P.derivative(self.test_xs, i), r[i])
def test_wrapper(self):
P = PiecewisePolynomial(self.xi, self.yi)
assert_almost_equal(P(self.test_xs), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs))
assert_almost_equal(
P.derivative(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=2)
)
assert_almost_equal(
P.derivatives(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=[0, 1])
)
def compute_positions_velocities(self, times, x_positions_velocities, y_positions_velocities, z_positions_velocities):
xinterpolator = PiecewisePolynomial(times, x_positions_velocities, orders=3, direction=1)
yinterpolator = PiecewisePolynomial(times, y_positions_velocities, orders=3, direction=1)
zinterpolator = PiecewisePolynomial(times, z_positions_velocities, orders=3, direction=1)
T = np.arange(0, times[-1], self.dt)
n = len(T)
precomputed_positions = np.empty((3,n))
precomputed_positions[0,:] = xinterpolator(T)
precomputed_positions[1,:] = yinterpolator(T)
precomputed_positions[2,:] = zinterpolator(T)
precomputed_velocities = np.empty((3,n))
precomputed_velocities[0,:] = xinterpolator.derivative(T)
precomputed_velocities[1,:] = yinterpolator.derivative(T)
precomputed_velocities[2,:] = zinterpolator.derivative(T)
return T, precomputed_positions, precomputed_velocities, n
def test_derivatives(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi, self.yi, 3)
m = 4
r = P.derivatives(self.test_xs, m)
#print r.shape, r
for i in xrange(m):
assert_almost_equal(P.derivative(self.test_xs, i), r[i])
def test_vector(self):
xs = [0, 1, 2]
ys = [[[0, 1]], [[1, 0], [-1, -1]], [[2, 1]]]
P = PiecewisePolynomial(xs, ys)
Pi = [PiecewisePolynomial(xs, [[yd[i] for yd in y] for y in ys]) for i in xrange(len(ys[0][0]))]
test_xs = np.linspace(-1, 3, 100)
assert_almost_equal(P(test_xs), np.rollaxis(np.asarray([p(test_xs) for p in Pi]), -1))
assert_almost_equal(
P.derivative(test_xs, 1), np.transpose(np.asarray([p.derivative(test_xs, 1) for p in Pi]), (1, 0))
)
def test_derivatives(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi,self.yi,3)
m = 4
r = P.derivatives(self.test_xs,m)
#print r.shape, r
for i in xrange(m):
assert_almost_equal(P.derivative(self.test_xs,i),r[i])
def test_wrapper(self):
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=DeprecationWarning)
P = PiecewisePolynomial(self.xi, self.yi)
assert_almost_equal(P(self.test_xs), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs))
assert_almost_equal(
P.derivative(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=2)
)
assert_almost_equal(
P.derivatives(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=[0, 1])
)
def test_vector(self):
xs = [0, 1, 2]
ys = [[[0,1]],[[1,0],[-1,-1]],[[2,1]]]
P = PiecewisePolynomial(xs,ys)
Pi = [PiecewisePolynomial(xs,[[yd[i] for yd in y] for y in ys])
for i in xrange(len(ys[0][0]))]
test_xs = np.linspace(-1,3,100)
assert_almost_equal(P(test_xs),
np.rollaxis(np.asarray([p(test_xs) for p in Pi]),-1))
assert_almost_equal(P.derivative(test_xs,1),
np.transpose(np.asarray([p.derivative(test_xs,1) for p in Pi]),
(1,0)))
def test_vector(self):
xs = [0, 1, 2]
ys = [[[0, 1]], [[1, 0], [-1, -1]], [[2, 1]]]
with warnings.catch_warnings():
warnings.filterwarnings("ignore", category=DeprecationWarning)
P = PiecewisePolynomial(xs, ys)
Pi = [PiecewisePolynomial(xs, [[yd[i] for yd in y] for y in ys]) for i in xrange(len(ys[0][0]))]
test_xs = np.linspace(-1, 3, 100)
assert_almost_equal(P(test_xs), np.rollaxis(np.asarray([p(test_xs) for p in Pi]), -1))
assert_almost_equal(
P.derivative(test_xs, 1), np.transpose(np.asarray([p.derivative(test_xs, 1) for p in Pi]), (1, 0))
)
def test_vector(self):
xs = [0, 1, 2]
ys = [[[0,1]],[[1,0],[-1,-1]],[[2,1]]]
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(xs,ys)
Pi = [PiecewisePolynomial(xs,[[yd[i] for yd in y] for y in ys])
for i in xrange(len(ys[0][0]))]
test_xs = np.linspace(-1,3,100)
assert_almost_equal(P(test_xs),
np.rollaxis(np.asarray([p(test_xs) for p in Pi]),-1))
assert_almost_equal(P.derivative(test_xs,1),
np.transpose(np.asarray([p.derivative(test_xs,1) for p in Pi]),
(1,0)))
def test_wrapper(self):
P = PiecewisePolynomial(self.xi, self.yi)
assert_almost_equal(
P(self.test_xs),
piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs))
assert_almost_equal(
P.derivative(self.test_xs, 2),
piecewise_polynomial_interpolate(self.xi,
self.yi,
self.test_xs,
der=2))
assert_almost_equal(
P.derivatives(self.test_xs, 2),
piecewise_polynomial_interpolate(self.xi,
self.yi,
self.test_xs,
der=[0, 1]))
def test_wrapper(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi,self.yi)
assert_almost_equal(P(self.test_xs),
piecewise_polynomial_interpolate(self.xi, self.yi,
self.test_xs))
assert_almost_equal(P.derivative(self.test_xs,2),
piecewise_polynomial_interpolate(self.xi,
self.yi,
self.test_xs,
der=2))
assert_almost_equal(P.derivatives(self.test_xs,2),
piecewise_polynomial_interpolate(self.xi,
self.yi,
self.test_xs,
der=[0,1]))
def test_derivative(self):
P = PiecewisePolynomial(self.xi, self.yi, 3)
assert_almost_equal(P.derivative(self.test_xs, 1), self.spline_yps)
def test_shapes_vectorvalue_derivative(self):
P = PiecewisePolynomial(self.xi, np.multiply.outer(self.yi, np.arange(3)), 4)
n = 4
assert_array_equal(np.shape(P.derivative(0, 1)), (3,))
assert_array_equal(np.shape(P.derivative([0], 1)), (1, 3))
assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, 3))
def move_trajectory(self, times, x_positions_velocities, y_positions_velocities, z_positions_velocities):
xinterpolator = PiecewisePolynomial(times, x_positions_velocities, orders=3, direction=1)
yinterpolator = PiecewisePolynomial(times, y_positions_velocities, orders=3, direction=1)
zinterpolator = PiecewisePolynomial(times, z_positions_velocities, orders=3, direction=1)
T = np.arange(0, times[-1], self.dt)
n = len(T)
velocity_and_w = np.zeros(6)
velocity_and_w[0] = xinterpolator.derivative(T[0])
velocity_and_w[1] = yinterpolator.derivative(T[0])
velocity_and_w[2] = zinterpolator.derivative(T[0])
precomputed_positions = np.empty((3,n))
precomputed_positions[0,:] = xinterpolator(T)
precomputed_positions[1,:] = yinterpolator(T)
precomputed_positions[2,:] = zinterpolator(T)
precomputed_velocities = np.empty((3,n))
precomputed_velocities[0,:] = xinterpolator.derivative(T)
precomputed_velocities[1,:] = yinterpolator.derivative(T)
precomputed_velocities[2,:] = zinterpolator.derivative(T)
actual_positions = np.empty((3,n))
actual_positions[:,0] = self.get_position()
corrector_velocities = np.empty((3,n))
corrector_velocities[:,0] = np.zeros(3)
proportional_velocities = np.empty((3,n))
proportional_velocities[:,0] = np.zeros(3)
integral_velocities = np.empty((3,n))
integral_velocities[:,0] = np.zeros(3)
derivative_velocities = np.empty((3,n))
derivative_velocities[:,0] = np.zeros(3)
vx_proportional, vy_proportional, vz_proportional = 0.0, 0.0, 0.0
last_vx_proportional, last_vy_proportional, last_vz_proportional = 0.0, 0.0, 0.0
vx_integral, vy_integral, vz_integral = 0.0, 0.0, 0.0
for i in xrange(1,n):
t_start = rospy.get_time()
self.limb.set_joint_velocities(
self.make_joint_dict(
self.get_joint_velocities(velocity_and_w)))
time_interval = T[i] - T[i-1]
position = self.get_position()
vx_proportional = precomputed_positions[0,i] - position[0]
vy_proportional = precomputed_positions[1,i] - position[1]
vz_proportional = precomputed_positions[2,i] - position[2]
proportional_velocities = self.kp * np.array([vx_proportional, vy_proportional, vz_proportional])
vx_integral += vx_proportional * time_interval
vy_integral += vy_proportional * time_interval
vz_integral += vz_proportional * time_interval
integral_velocities = self.ki * np.array([vx_integral, vy_integral, vz_integral])
vx_derivative = (vx_proportional - last_vx_proportional)/time_interval
vy_derivative = (vy_proportional - last_vy_proportional)/time_interval
vz_derivative = (vz_proportional - last_vz_proportional)/time_interval
derivative_velocities = self.kd * np.array([vx_derivative, vy_derivative, vz_derivative])
vx_corrector = self.kp * vx_proportional + self.ki * vx_integral + self.kd * vx_derivative
vy_corrector = self.kp * vy_proportional + self.ki * vy_integral + self.kd * vy_derivative
vz_corrector = self.kp * vz_proportional + self.ki * vz_integral + self.kd * vz_derivative
corrector_velocities[:,i] = np.array([vx_corrector, vy_corrector, vz_corrector])
velocity_and_w[0] = precomputed_velocities[0,i] + vx_corrector
velocity_and_w[1] = precomputed_velocities[1,i] + vy_corrector
velocity_and_w[2] = precomputed_velocities[2,i] + vz_corrector
last_vx_proportional = vx_proportional
last_vy_proportional = vy_proportional
last_vz_proportional = vz_proportional
actual_positions[:,i] = position
end_time = time_interval + t_start
loginfo("Computation Took: {0} out of {1} seconds".format(rospy.get_time() - t_start, time_interval))
rospy.sleep(end_time - rospy.get_time())
loginfo("exit_control_mode")
self.limb.exit_control_mode()
paramtext = "%1.4f_%1.4f_%1.4f_%1.4f_%1.4f" % (self.kp, self.ki, self.kd, self.extra_motion_maximum, self.extra_motion_multiple)
# date = ""
date = str(datetime.now())
folder = "tests"
A = np.empty((n,4))
A[:,0] = T
A[:,1:] = actual_positions.T
# np.savetxt("/home/cs4752/ros_ws/src/cs4752_proj2/{2}/{1}actual-positions-{0}.csv".format(paramtext,date,folder),A)
B = np.empty((n,4))
B[:,0] = T
B[:,1:] = precomputed_positions.T
# np.savetxt("/home/cs4752/ros_ws/src/cs4752_proj2/{2}/{1}precomputed-positions-{0}.csv".format(paramtext,date,folder),B)
loginfo("saved errors")
def test_derivative(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi,self.yi,3)
assert_almost_equal(P.derivative(self.test_xs,1),self.spline_yps)
def test_derivative(self):
P = PiecewisePolynomial(self.xi, self.yi, 3)
assert_almost_equal(P.derivative(self.test_xs, 1), self.spline_yps)
class FiberSource:
"""
A ``FiberSource`` is a continuous representation of a fiber trajectory,
.. math::
f: [0, 1] \\rightarrow \\mathcal{R}^3.
The trajectory is modeled by 3 piecewise polynomials (one for each
dimension). Note that the fiber is connecting two end points on the
"cortical surface". The construction makes sure that the tangents to the
fiber are normal to this surface.
Parameters
----------
control_points : array-like shape (nb_points, 3)
a set of points through which the fiber will go.
tangents : optional, default = 'symmetric'
Either 'symmetric', 'incoming', 'outgoing'. Controls the way the
tangents are computed.
scale : optional, default = 1.0
A multiplicative factor for the points positions. This corresponds to
the sphere radius in mm.
"""
def __init__(self, control_points, **kwargs):
# The mode to compute tangents, either 'symmetric', 'incoming', or
# 'outgoing'
tangents = kwargs.get('tangents', 'symmetric')
scale = kwargs.get('scale', 1.0)
self._create_from_control_points(control_points, tangents, scale)
def __call__(self, ts):
return self.interpolate(ts)
def _create_from_control_points(self, control_points, tangents, scale):
"""
Creates the FiberSource instance from control points, and a specified
mode to compute the tangents.
Parameters
----------
control_points : ndarray shape (N, 3)
tangents : 'incoming', 'outgoing', 'symmetric'
scale : multiplication factor.
This is useful when the coodinates are given dimensionless, and we
want a specific size for the phantom.
"""
# Compute instant points ts, from 0. to 1.
# (time interval proportional to distance between control points)
nb_points = control_points.shape[0]
dists = np.zeros(nb_points)
dists[1:] = np.sqrt((np.diff(control_points, axis=0) ** 2).sum(1))
ts = dists.cumsum()
length = ts[-1]
ts = ts / np.max(ts)
# Create interpolation functions (piecewise polynomials) for x, y and z
derivatives = np.zeros((nb_points, 3))
# The derivatives at starting and ending points are normal
# to the surface of a sphere.
derivatives[0, :] = -control_points[0]
derivatives[-1, :] = control_points[-1]
# As for other derivatives, we use discrete approx
if tangents == 'incoming':
derivatives[1:-1, :] = (control_points[1:-1] - control_points[:-2])
elif tangents == 'outgoing':
derivatives[1:-1, :] = (control_points[2:] - control_points[1:-1])
elif tangents == 'symmetric':
derivatives[1:-1, :] = (control_points[2:] - control_points[:-2])
else:
raise Error('tangents should be one of the following: incoming, '
'outgoing, symmetric')
derivatives = (derivatives.T / np.sqrt((derivatives ** 2).sum(1))).T \
* length
self.x_poly = PiecewisePolynomial(ts,
scale * np.vstack((control_points[:, 0], derivatives[:, 0])).T)
self.y_poly = PiecewisePolynomial(ts,
scale * np.vstack((control_points[:, 1], derivatives[:, 1])).T)
self.z_poly = PiecewisePolynomial(ts,
scale * np.vstack((control_points[:, 2], derivatives[:, 2])).T)
def interpolate(self, ts):
"""
From a ``FiberSource``, which is a continuous representation, to a
``Fiber``, a discretization of the fiber trajectory.
Parameters
----------
ts : array-like, shape (N, )
A list of "timesteps" between 0 and 1.
Returns
-------
trajectory : array-like, shape (N, 3)
The trajectory of the fiber, discretized over the provided
timesteps.
"""
N = ts.shape[0]
trajectory = np.zeros((N, 3))
trajectory[:, 0] = self.x_poly(ts)
trajectory[:, 1] = self.y_poly(ts)
trajectory[:, 2] = self.z_poly(ts)
return trajectory
def tangents(self, ts):
"""
Get tangents (as unit vectors) at given timesteps.
Parameters
----------
ts : array-like, shape (N, )
A list of "timesteps" between 0 and 1.
Returns
-------
tangents : array-like, shape (N, 3)
The tangents (as unit vectors) to the fiber at selected timesteps.
"""
x_der = self.x_poly.derivative(ts, der=1)
y_der = self.y_poly.derivative(ts, der=1)
z_der = self.z_poly.derivative(ts, der=1)
N = ts.shape[0]
tangents = np.zeros((N, 3))
tangents[:, 0] = x_der
tangents[:, 1] = y_der
tangents[:, 2] = z_der
tangents = tangents / np.sqrt(np.sum(tangents ** 2, 1))[:, np.newaxis]
return tangents
def curvature(self, ts):
"""
Evaluates the curvature of the fiber at given positions. The curvature
is computed with the formula
.. math::
\gamma = \\frac{\|f'\wedge f''\|}{\|f'\|^3}\qquad.
Parameters
----------
ts : array-like, shape (N, )
A list of "timesteps" between 0 and 1.
Returns
-------
curvatures : array-like, shape (N, )
The curvatures of the fiber trajectory, at selected timesteps.
"""
x_der1 = self.x_poly.derivative(ts, der=1)
x_der2 = self.x_poly.derivative(ts, der=2)
y_der1 = self.y_poly.derivative(ts, der=1)
y_der2 = self.y_poly.derivative(ts, der=2)
z_der1 = self.z_poly.derivative(ts, der=1)
z_der2 = self.z_poly.derivative(ts, der=2)
curv = (z_der2*y_der1 - y_der2*z_der1)**2
curv += (x_der2*z_der1 - z_der2*x_der1)**2
curv += (y_der2*x_der1 - x_der2*y_der1)**2
curv /= (x_der1**2 + y_der1**2 + z_der1**2)**3
np.sqrt(curv, out=curv)
return curv
def test_derivative(self):
with warnings.catch_warnings():
warnings.filterwarnings('ignore', category=DeprecationWarning)
P = PiecewisePolynomial(self.xi, self.yi, 3)
assert_almost_equal(P.derivative(self.test_xs, 1), self.spline_yps)
