-
Notifications
You must be signed in to change notification settings - Fork 55
/
utils_reg.py
167 lines (146 loc) · 6.52 KB
/
utils_reg.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
from __future__ import division
import airsim
from airsim.utils import to_eularian_angles, to_quaternion
import numpy as np
from airsim.types import Pose, Vector3r, Quaternionr
import nav_msgs.msg
import geometry_msgs.msg
import tf2_ros
import tf_conversions
import tf2_msgs
import tf2_py
import tf
import tf.msg
from tf import transformations
import rospy
import time
import random
import math
def polarTranslation(r, theta, psi):
# follow math convention for polar coordinates
# r: radius
# theta: azimuth (horizontal)
# psi: vertical
x = r * np.cos(theta) * np.sin(psi)
y = r * np.sin(theta) * np.sin(psi)
z = r * np.cos(psi)
return Vector3r(x, y, z)
def convert_gate_base2world(p_o_b, t_b_g, phi_rel):
# transform relative vector from base frame to the world frame
m = geometry_msgs.msg.TransformStamped()
m.header.frame_id = 'world_frame'
m.header.stamp = rospy.Time(10)
m.child_frame_id = 'base_frame'
m.transform.translation.x = p_o_b.position
m.transform.rotation.x = p_o_b.orientation.x_val
m.transform.rotation.y = p_o_b.orientation.y_val
m.transform.rotation.z = p_o_b.orientation.z_val
m.transform.rotation.w = p_o_b.orientation.w_val
m.transform.translation.x = p_o_b.position.x_val
m.transform.translation.y = p_o_b.position.y_val
m.transform.translation.z = p_o_b.position.z_val
point = geometry_msgs.msg.PointStamped()
point.point.x = t_b_g.x_val
point.point.y = t_b_g.y_val
point.point.z = t_b_g.z_val
point.header.frame_id = 'base_frame'
point.header.stamp = rospy.Time(10) # set an arbitrary time instance
t = tf.TransformerROS()
t.setTransform(m)
# finally we get the gate coord in world coordinates
point_converted = t.transformPoint('world_frame', point)
t_o_g = Vector3r(point_converted.point.x, point_converted.point.y, point_converted.point.z) # now as a world coord vector
# create rotation of gate
# find vector t_b_g in world coordinates
t_b_g = t_o_g - p_o_b.position
phi_quad_ref = np.arctan2(t_b_g.y_val, t_b_g.x_val)
phi_gate = phi_quad_ref + phi_rel
q = transformations.quaternion_from_euler(phi_gate, 0, 0, axes='rzyx')
p_o_g = Pose(t_o_g, Quaternionr(q[0], q[1], q[2], q[3]))
return p_o_g
def MoveCheckeredGates(client):
gate_names_sorted = sorted(client.simListSceneObjects("Gate.*"))
# gate_names_sorted_bad is ['Gate0', 'Gate10_21', 'Gate11_23', 'Gate1_3', 'Gate2_5', 'Gate3_7', 'Gate4_9', 'Gate5_11', 'Gate6_13', 'Gate7_15', 'Gate8_17', 'Gate9_19']
# number after underscore is unreal garbage
pose_far = Pose(Vector3r(0,0,1), Quaternionr())
for gate in gate_names_sorted:
client.simSetObjectPose(gate, pose_far)
# time.sleep(0.05)
def AllGatesDestroyer(client):
[client.simDestroyObject(gate_object) for gate_object in client.simListSceneObjects(".*[Gg]ate.*")]
def RedGateSpawner(client, num_gates, noise_amp):
gate_poses=[]
for idx in range(num_gates):
noise = (np.random.random()-0.5)*noise_amp
pose = Pose(Vector3r(10+idx*9, noise*5.0, 10.0), Quaternionr(0.0, 0.0, 0.707, 0.707))
client.simSpawnObject("gate_"+str(idx), "RedGate16x16", pose, 1.5)
gate_poses.append(pose)
return gate_poses
def RedGateSpawnerCircle(client, num_gates, radius, radius_noise, height_range):
track = generate_gate_poses(num_gates=num_gates, race_course_radius=radius, radius_noise=radius_noise, height_range=height_range)
for idx in range(num_gates):
client.simSpawnObject("gate_" + str(idx), "RedGate16x16", track[idx], 1.5)
def generate_gate_poses(num_gates, race_course_radius, radius_noise, height_range, type_of_segment="circle"):
if type_of_segment == "circle":
(x_t, y_t, z_t) = tuple([generate_circle(i, num_gates, race_course_radius, radius_noise) for i in range(3)])
# todo unreadable code
# todo un-hardcode
# airsim.Vector3r((x_t[t_i][0] - x_t[0][0]), (y_t[t_i][0] - y_t[0][0]), random.uniform(height_range[0], height_range[1])), \
gate_poses = [\
airsim.Pose(\
airsim.Vector3r((x_t[t_i][0]), (y_t[t_i][0]), random.uniform(height_range[0], height_range[1])), \
quaternionFromUnitGradient(x_t[t_i][1], y_t[t_i][1], z_t[t_i][1])\
)\
for t_i in range(num_gates)]
# elif type_of_segment == "cubic":
return gate_poses
def quaternionFromUnitGradient(dx_dt, dy_dt, dz_dt):
default_gate_facing_vector = type("", (), dict(x=0, y=1, z=0))()
r0 = default_gate_facing_vector
q = airsim.Quaternionr(
r0.y * dz_dt - r0.z * dy_dt,
r0.z * dx_dt - r0.x * dz_dt,
r0.x * dy_dt - r0.y * dx_dt,
math.sqrt((r0.x**2 + r0.y**2 + r0.z**2) * (dx_dt**2 + dy_dt**2 + dz_dt**2)) + (r0.x * dx_dt + r0.y * dy_dt + r0.z * dz_dt)
)
#Normalize
length = q.get_length()
if (length == 0.0):
q.w_val = 1.0
else:
q.w_val /= length
q.x_val /= length
q.y_val /= length
q.z_val /= length
return q
def generate_circle(i, num_gates, race_course_radius, radius_amp=4.0):
ts = [t / (num_gates) for t in range(0, num_gates)]
samples = [0 for t in ts]
derivatives = [0 for t in ts]
min_radius = race_course_radius + radius_amp
max_radius = race_course_radius - radius_amp
max_radius_delta = 5.0
radius_list = [random.uniform(min_radius, max_radius) for t in ts]
# not a circle, but hey it's random-ish. and the wrong derivative actually make the track challenging
# come back again later.
if i == 0:
for (idx, t) in enumerate(ts):
radius = radius_list[idx]
if idx > 0:
radius = np.clip(radius, radius_list[idx-1] - max_radius_delta, radius_list[idx-1] + max_radius_delta)
radius = np.clip(radius, 0.0, radius)
samples[idx] = radius * math.cos(2.*math.pi * t)
derivatives[idx] = radius * -math.sin(2.*math.pi * t)
elif i == 1:
for (idx, t) in enumerate(ts):
radius = radius_list[idx]
if idx > 0:
radius = np.clip(radius, radius_list[idx-1] - max_radius_delta, radius_list[idx-1] + max_radius_delta)
radius = np.clip(radius, 0.0, radius)
samples[idx] = radius * math.sin(2.*math.pi * t)
derivatives[idx] = radius * math.cos(2.*math.pi * t)
else:
for (idx, t) in enumerate(ts):
samples[idx] = 0.
derivatives[idx] = 0.
return list(zip(samples, derivatives))