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_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_incremental(self):
        with warnings.catch_warnings():
            warnings.filterwarnings('ignore', category=DeprecationWarning)
            P = PiecewisePolynomial([self.xi[0]], [self.yi[0]], 3)

        for i in xrange(1,len(self.xi)):
            P.append(self.xi[i],self.yi[i],3)
        assert_almost_equal(P(self.test_xs),self.spline_ys)

 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_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_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_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_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]]]
        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 _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 __init__(self, x, y, axis=0):
        x = np.asarray(x)
        y = np.asarray(y)

        axis = axis % y.ndim

        xp = x.reshape((x.shape[0],) + (1,)*(y.ndim-1))
        yp = np.rollaxis(y, axis)

        data = np.empty((yp.shape[0], 2) + yp.shape[1:], y.dtype)
        data[:,0] = yp
        data[:,1] = self._get_derivatives(xp, yp)

        s = list(range(2, y.ndim + 1))
        s.insert(axis, 1)
        s.insert(axis, 0)
        data = data.transpose(s)

        PiecewisePolynomial.__init__(self, x, data, orders=3, direction=None,
                                     axis=axis)

    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_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 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))
#.........这里部分代码省略.........

 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_derivative(self):
     P = PiecewisePolynomial(self.xi, self.yi, 3)
     assert_almost_equal(P.derivative(self.test_xs, 1), self.spline_yps)

 def test_incremental(self):
     P = PiecewisePolynomial([self.xi[0]], [self.yi[0]], 3)
     for i in xrange(1, len(self.xi)):
         P.append(self.xi[i], self.yi[i], 3)
     assert_almost_equal(P(self.test_xs), self.spline_ys)