def getObjectPose(self, name, ndims=2):
bodyId = self.objects[name]['bodyId']
pos, rot = self._p.getBasePositionAndOrientation(bodyId)
if ndims == 2:
q_offset = self.objects[name]['shape']['q_offset']
q_offset = quaternion.Quaternion(x=q_offset[0],
y=q_offset[1],
z=q_offset[2],
w=q_offset[3]).normalised
q_rot = quaternion.Quaternion(x=rot[0],
y=rot[1],
z=rot[2],
w=rot[3]).normalised
q_rot = (q_rot * q_offset.inverse).normalised
(_, _, yaw) = quaterion_to_euler(q_rot)
x = pos[0]
y = pos[1]
yaw = distance_angle(yaw, 0)
return x, y, yaw
else:
q_rot = quaternion.Quaternion(x=rot[0],
y=rot[1],
z=rot[2],
w=rot[3]).normalised
return pos, q_rot
def move_and_rotate(self, name, pos_delta, yaw_delta):
bodyId = self.objects[name]['bodyId']
pos_delta = np.array(pos_delta)
rot_delta = self._p.getQuaternionFromEuler([0, 0, yaw_delta])
rot_delta = quaternion.Quaternion(x=rot_delta[0],
y=rot_delta[1],
z=rot_delta[2],
w=rot_delta[3]).normalised
pos_curr, rot_curr = self._p.getBasePositionAndOrientation(bodyId)
pos_curr = np.array(pos_curr)
rot_curr = quaternion.Quaternion(x=rot_curr[0],
y=rot_curr[1],
z=rot_curr[2],
w=rot_curr[3]).normalised
pos_goal = pos_delta + pos_curr
rot_goal = rot_delta * rot_curr
rot_goal = rot_goal.normalised
# 20 degree per second OR 3cm per second
dist = np.linalg.norm(pos_delta)
time_pos = dist / 0.01
time_rot = np.abs(yaw_delta) * (180.0 / 20.0) / np.pi
time_req = max(max(time_pos, time_rot), 0.1)
time_delta = 0.1
n_iters = np.round(time_req / time_delta)
pos_per_iter = list(np.array(pos_delta) / float(n_iters))
rot_per_iter = self._p.getQuaternionFromEuler(
[0, 0, yaw_delta / float(n_iters)])
rot_per_iter = quaternion.Quaternion(x=rot_per_iter[0],
y=rot_per_iter[1],
z=rot_per_iter[2],
w=rot_per_iter[3]).normalised
for i in range(int(n_iters)):
pos_curr = pos_per_iter + pos_curr
rot_curr = rot_per_iter * rot_curr
rot_curr = rot_curr.normalised
rot = list(rot_curr.q[1:]) + [rot_curr.q[0]]
self._p.resetBasePositionAndOrientation(bodyId, pos_curr, rot)
self._p.stepSimulation()
time.sleep(0.01)
def dcm2q(rnb):
q8 = np.zeros(4)
q8[3] = 0.5 * np.sqrt(np.abs(1 + np.sum(np.diag(rnb))))
q8[0] = (rnb[2, 1] - rnb[1, 2]) / (4 * q8[3])
q8[1] = (rnb[0, 2] - rnb[2, 0]) / (4 * q8[3])
q8[2] = (rnb[1, 0] - rnb[0, 1]) / (4 * q8[3])
return q.Quaternion(q8)
def __init__(self):
self.mass = 1 # kg
self.center_of_gravity = np.zeros(3)
self.moment_of_inertia = np.identity(3) #relative to cog
self.position = np.zeros(3)
self.rotation = quaternion.Quaternion()
self.velocity = np.zeros(3)
def getPosRot(self, shape, x, y, yaw):
z_offset = shape['z_offset']
q_offset = shape['q_offset']
q_offset = quaternion.Quaternion(x=q_offset[0],
y=q_offset[1],
z=q_offset[2],
w=q_offset[3]).normalised
pos = [x, y, z_offset]
q_delta = self._p.getQuaternionFromEuler([0, 0, yaw])
q_delta = quaternion.Quaternion(x=q_delta[0],
y=q_delta[1],
z=q_delta[2],
w=q_delta[3]).normalised
rot = q_delta * q_offset
rot = list(rot.q[1:]) + [rot.q[0]]
return pos, rot
def nav_eq(x, u, ts):
g_t = np.array(gravity(), ndmin=2).transpose()
x_q = q.Quaternion(x[6], x[7], x[8], x[9])
x_main = np.array(x[0:6], ndmin=2)
f_t = np.matmul(q2dcm(x_q), u[0:3])
acc_t = f_t - g_t
# state space model matrices
a = np.eye(6)
a[0, 3] = ts
a[1, 4] = ts
a[2, 5] = ts
# matrix form of kinematic equations used to calculate progress
b_1 = np.append(np.eye(3), np.zeros((3, 3)), axis=0) * (ts**2 / 2)
b_2 = np.append(np.zeros((3, 3)), np.eye(3), axis=0) * ts
b = b_1 + b_2
x_1 = np.array(np.matmul(a, x_main))
x_2 = np.matmul(b, acc_t)
x[0:6] = x_1 + x_2
w_tb = u[3:6]
P = w_tb[0] * ts
Q = w_tb[1] * ts
R = w_tb[2] * ts
# OMEGA matrix definition as described in reference and final report
omega = np.zeros((4, 4))
omega[0, 0:4] = [0, R * 0.5, -Q * 0.5, P * 0.5]
omega[1, 0:4] = [-R * 0.5, 0, P * 0.5, Q * 0.5]
omega[2, 0:4] = [Q * 0.5, -P * 0.5, 0, R * 0.5]
omega[3, 0:4] = [-P * 0.5, -Q * 0.5, -R * 0.5, 0]
v = np.linalg.norm(w_tb) * ts
if v != 0:
x[6:10] = np.matmul(
(np.cos(v / 2) * np.eye(4) + 2 / v * np.sin(v / 2) * omega),
x[6:10])
return x
def __init__(self):
super().__init__(self)
self.position = np.zeros(3)
self.rotation = quaternion.Quaternion()
