Python PiecewisePolynomial.derivative方法代码示例

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
 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,))

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
 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])

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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))

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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])

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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,))

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
 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])

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
 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])
     )

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
 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))
     )

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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])

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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])
        )

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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))
        )

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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)

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
 def test_derivative(self):
     P = PiecewisePolynomial(self.xi, self.yi, 3)
     assert_almost_equal(P.derivative(self.test_xs, 1), self.spline_yps)

# 需要导入模块: from scipy.interpolate import PiecewisePolynomial [as 别名]
# 或者: from scipy.interpolate.PiecewisePolynomial import derivative [as 别名]
    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))
#.........这里部分代码省略.........