def test_matrix2numpy_conversion():
a = Matrix([[1, 2, sin(x)], [x**2, x, Rational(1, 2)]])
b = array([[1, 2, sin(x)], [x**2, x, Rational(1, 2)]])
assert (matrix2numpy(a) == b).all()
assert matrix2numpy(a).dtype == numpy.dtype('object')
c = matrix2numpy(Matrix([[1, 2], [10, 20]]), dtype='int8')
d = matrix2numpy(Matrix([[1, 2], [10, 20]]), dtype='float64')
assert c.dtype == numpy.dtype('int8')
assert d.dtype == numpy.dtype('float64')
def getl_ultrasonic_side(self, convert_to_float=False):
self.new_world()
a = self.world.getl_ultrasonic_side()
if convert_to_float:
a = dense.matrix2numpy(a, dtype=float)
a = a.flatten()
return a
def getl_ultrasonic_top(self, float=False):
self.new_world()
a = self.world.getl_ultrasonic_top()
if float:
a = dense.matrix2numpy(a, dtype=float)
a = a.flatten()
return a
def get_rvec(self, coordsys, base):
rot_mat = base.rotation_matrix(coordsys)
rot_mat_np = dense.matrix2numpy(rot_mat, dtype=float)
rotation_vector, _ = cv2.Rodrigues(rot_mat_np)
rotation_vector = rotation_vector.flatten()
return rotation_vector
def test_matrix2numpy():
a = matrix2numpy(Matrix([[1, x**2], [3*sin(x), 0]]))
assert isinstance(a, ndarray)
assert a.shape == (2, 2)
assert a[0, 0] == 1
assert a[0, 1] == x**2
assert a[1, 0] == 3*sin(x)
assert a[1, 1] == 0
def newton(self, max_ite=1000):
# 初始化
setattr(self, "ite", 0)
setattr(self, "x1", self.x1_init)
setattr(self, "x2", self.x2_init)
fx = diff(self.f, self.x)
fy = diff(self.f, self.y)
grad_f1 = Matrix([[fx], [fy]])
grad_H2 = hessian(self.f, (self.x, self.y))
x_tmp = self.x1_init
y_tmp = self.x2_init
#迭代
# start = time.time()
for _ in range(max_ite):
grad_f1 = np.array(
[[float(fx.evalf(subs={
self.x: x_tmp,
self.y: y_tmp
}))], [float(fy.evalf(subs={
self.x: x_tmp,
self.y: y_tmp
}))]])
tmp = matrix2numpy(grad_H2.evalf(subs={
self.x: x_tmp,
self.y: y_tmp
}),
dtype=float)
ans_tmp = np.array([[x_tmp], [y_tmp]]) - np.dot(
np.linalg.inv(tmp), grad_f1)
acc_tmp = ((ans_tmp[0, 0] - x_tmp)**2 +
(ans_tmp[1, 0] - y_tmp)**2)**0.5
self.ite += 1
print("第{}次迭代后,坐标为({}, {})".format(self.ite, ans_tmp[0, 0],
ans_tmp[1, 0]))
x_tmp = ans_tmp[0, 0]
y_tmp = ans_tmp[1, 0]
f_tmp = self.f.evalf(subs={self.x: x_tmp, self.y: y_tmp})
if acc_tmp <= self.stop_condition:
self.x1 = ans_tmp[0, 0]
self.x2 = ans_tmp[1, 0]
break
# end = time.time()
# print((end-start)*1000)
print("\n迭代结束点为({}, {})".format(self.x1, self.x2))
print("最小值为{}".format(
self.f.evalf(subs={
self.x: self.x1,
self.y: self.x2
})))
def find_fixed_point(self, r_init):
"""
Returns the value of a fixed point based a Newton's method computation.
See https://en.wikipedia.org/wiki/Newton%27s_method for details.
"""
num_iter = 50
r = r_init
for _ in range(num_iter):
J = self.eval_jacobian(r)
J_inv = matrix2numpy(J.inv(), dtype="float")
r = r - J_inv.dot(self.phasespace_eval(t=None, r=r))
self.fixed_points.add(r)
return r
def get_numpy_mat(vec: sympy.vector.vector.Vector, base):
a = vec.to_matrix(base)
# a = DummyVariable.eval_all(a)
b = dense.matrix2numpy(a, dtype=float)
return b
reaching_states = np.where(np.logical_and(x_s_distance != 0, x_s_distance < np.inf))[0]
# List of state indices from which the state x_s is unreachable.
non_reaching_states = np.where(x_s_distance == np.inf)[0]
# r×r sub-matrix of P, transition probability from each reaching state to each reaching state
P_1 = P[np.ix_(reaching_states, reaching_states)]
# r×1 sub-matrix of P, transition probability from each reaching state to state x_s
P_2 = P[np.ix_(reaching_states, [x_s])]
# Identity matrix
I = np.identity(len(reaching_states))
# Hitting probability from any reaching state to x_s, given by the system of linear equations h = h*P_1.T + P_2.T
h_reaching = P_2.T * matrix2numpy(sympyMatrix(I - P_1.T).inv())
h = np.zeros(len(P), dtype=sympy.symbol.Symbol)
h[reaching_states] = h_reaching
h[x_s] = 1.
# Probability of reaching x_s starting from x_i
h_i = h[x_i]
# Compute partial derivatites for each parameter
print("\nSensitivity and its absolute value, for each parameter:")
sensitivity = dict()
for p in parameters.keys():
sensitivity[p] = diff(h_i, p).subs(parameters)
print(str(p).ljust(10, ' '), '%0.5f\t%0.5f' % (float(sensitivity[p]), np.abs(float(sensitivity[p]))))
theta = aruco_angle
x, y, z = 0.485, -0.295, 0.456 # coordinates of camera in respect to robot coord [m]
a, b, c = aruco_x, aruco_y, aruco_z
Robot = CoordSys3D('Robot')
Cam = Robot.locate_new('Cam', x * Robot.i + y * Robot.j + z * Robot.k)
Cam2 = Cam.orient_new_axis('Cam2', theta, Cam.i)
Marker = Cam2.locate_new('Marker', a * Cam2.i + b * Cam2.j + c * Cam2.k)
mat = Marker.position_wrt(Robot).to_matrix(Robot)
# mat = Robot.position_wrt(Marker).to_matrix(Marker)
from sympy.matrices import dense
mat = dense.matrix2numpy(mat, float)
print(mat)
MarkerTurned = Marker.orient_new_axis('MarkerTurned', aruco_angle, Marker.k)
#
# print(Cam.position_wrt(Robot))
# print(Robot.origin.express_coordinates(Cam2))
# print(Cam.origin.express_coordinates(Robot))
# print(Robot.origin.express_coordinates(Marker))
# print(Marker.origin.express_coordinates(Robot))
# print(MarkerTurned.rotation_matrix(Robot))
# print(express(MarkerTurned.position_wrt(Robot), MarkerTurned))
from matplotlib import pyplot
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import matplotlib as plt