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)