def calc_scan_points(angles):
[a04, q5, q7] = sensor_location(angles[0], angles[1], angles[2], angles[3],
angles[4])
plain_offset = -l4
x7 = np.array(range(-25, 27, 2))
y7 = np.array(range(-25, 27, 2))
# xx7, yy7 = np.meshgrid(x7, y7)
z7 = plain_offset * np.ones(len(y7))
size = len(y7)**2
scan_matrix = list(
np.array([np.ones(size),
np.ones(size),
np.ones(size),
np.ones(size)]))
scan_matrix[-1][-1] = 1
i = 0
j = 0
index = 0
while index < size:
scan_matrix[0][index] = x7[i]
scan_matrix[1][index] = y7[j]
scan_matrix[2][index] = z7[i]
i += 1
index += 1
if x7[i] == -25 or x7[i] == 25:
scan_matrix[0][index] = x7[i]
scan_matrix[1][index] = y7[j]
scan_matrix[2][index] = z7[i]
x7 = np.flipud(x7)
j += 1
index += 1
i = 0 # ??????
world_point_mat = a04.dot(scan_matrix)
# print(world_point_mat)
# calc World's points
h = 0
world_angles = (np.array([np.zeros(size), np.zeros(size), np.zeros(size)]))
while h < len(world_point_mat[0]):
some_angles = joints_angles(world_point_mat[0][h],
world_point_mat[1][h],
world_point_mat[2][h])
world_angles[0][h] = some_angles[0]
world_angles[1][h] = some_angles[1]
world_angles[2][h] = some_angles[2]
h += 1
# world_angles = np.transpose(world_angles)
return world_angles
def joints_angle_read(data):
global POS2RAD
global ZERO_OFFSET
global joints_angles_rad
rospy.loginfo(rospy.get_caller_id() + ' \n --Got the joints angles: %f %f %f', data.data[0], data.data[1], data.data[2])
joints_angles_raw = data.data
print(joints_angles_raw)
joints_angles_rad[0] = (joints_angles_raw[0] - ZERO_OFFSET) * POS2RAD
joints_angles_rad[1] = (joints_angles_raw[1] - ZERO_OFFSET) * POS2RAD
joints_angles_rad[2] = (joints_angles_raw[2] - ZERO_OFFSET) * POS2RAD
print(joints_angles_rad)
# data.data[0], data.data[1], data.data[2]
location.data = sensor_location(joints_angles_rad[0], joints_angles_rad[1], joints_angles_rad[2], alpha, beta)
rospy.loginfo('location: %f %f %f', location.data[0], location.data[1], location.data[2])
pub_location.publish(location)
def joints_angle_read(data):
global POS2RAD
global ZERO_OFFSET
global joints_angles_rad
global temp
global q4
temp = data.data
joints_angles_rad = list(data.data)
joints_angles_rad[0] = (data.data[0] - ZERO_OFFSET) * POS2RAD # <TODO> modulo or subtraction ?
joints_angles_rad[1] = (data.data[1] - ZERO_OFFSET) * POS2RAD
joints_angles_rad[2] = (data.data[2] - ZERO_OFFSET) * POS2RAD
joints_angles_rad[3] = data.data[3] * DEG2RAD
joints_angles_rad[4] = data.data[4] * DEG2RAD
[a04, q4, q5] = sensor_location(joints_angles_rad[0], joints_angles_rad[1],
joints_angles_rad[2], joints_angles_rad[3], joints_angles_rad[4])
def scan_node():
global angles
global g
global h
rospy.init_node('scan_node', anonymous=True)
pub1 = rospy.Publisher("write/angles", Float32MultiArray, queue_size=10)
#rospy.Subscriber('read/angles_read', Float32MultiArray, joints_angle_read)
rospy.Subscriber('read/alpha', Float32, alpha)
rospy.Subscriber('read/beta', Float32, beta)
# rospy.Subscriber('load', Float32MultiArray, beta) # for tourqe calc
# Creates the scan matrix
plain_offset = -l4
x7 = (range(-25, 25, delta))
y7 = (range(25, -25, -delta))
z7 = plain_offset * np.ones(len(y7))
for_dim = np.zeros(len(y7))
for_dim[-1] = 1
scan_matrix = np.array([x7, y7, z7, for_dim])
#[a04, q5, q7] = sensor_location(joints_angles_rad[0], joints_angles_rad[1], joints_angles_rad[2], g, 0)
# world_points = a04.dot(scan_matrix)
print(g)
# calc World's points
i = 0
angles_mat = list(np.ones(len(y7)))
while i < len(y7):
#some_angles = joints_angles(world_points[0, i], world_points[1, i], world_points[2, i])
#angles_mat[i] = some_angles
i += 1
angles_mat = np.transpose(angles_mat)
#rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if g:
[a04, q5, q7] = sensor_location(joints_angles_rad[0], joints_angles_rad[1], joints_angles_rad[2], g, 0)
print(a04)
print('\n')
print(g)
print(h)
print('\n')
#rate.sleep()
rospy.spin()
import numpy as np import math from DirectKinmatics import sensor_location from InverseKinmatics import joints_angles DEG2RAD = math.pi / 180 l4 = 31.5 angles = np.array([60, 60, 90, 0, 0]) * DEG2RAD # Insert in degree [a04, q4, q5] = sensor_location(angles[0], angles[1], angles[2], angles[3], angles[4]) print(np.linalg.norm(q5-q4)) plain_offset = -l4 x7 = np.array(range(-20, 22, 2)) y7 = np.array(range(-20, 22, 2)) z71 = plain_offset * np.ones(len(y7)) z72 = (plain_offset-2) * np.ones(len(y7)) # z7 = plain_offset * np.ones(len(y7)) size = len(y7) ** 2 scan_matrix = list(np.array([np.ones(size), np.ones(size), np.ones(size), np.ones(size)])) i = 0 j = 0 index = 0 while index < size: