def _distance_from_fixed_angle(angle, fixed_angle):
"""Designed to be mapped, this function finds the smallest rotation between
two rotations. It assumes a no-symmettry system.
The algorithm involves converting angles to quarternions, then finding the
appropriate misorientation. It is tested against the slower more complete
version finding the joining rotation.
Parameters
----------
angle : np.array()
Euler angles representing rotation of interest.
fixed_angle : np.array()
Euler angles representing fixed reference rotation.
Returns
-------
theta : np.array()
Rotation angle between angle and fixed_angle.
"""
angle = angle[0]
q_data = euler2quat(*np.deg2rad(angle), axes='rzxz')
q_fixed = euler2quat(*np.deg2rad(fixed_angle), axes='rzxz')
if np.abs(2 * (np.dot(q_data, q_fixed))**2 - 1) < 1: # arcos will work
# https://math.stackexchange.com/questions/90081/quaternion-distance
theta = np.arccos(2 * (np.dot(q_data, q_fixed))**2 - 1)
else: # slower, but also good
q_from_mode = qmult(qinverse(q_fixed), q_data)
axis, theta = quat2axangle(q_from_mode)
theta = np.abs(theta)
return np.rad2deg(theta)
def get_quaternion_training_data_from_bvh(bvh_filename):
data = parse_frames(bvh_filename)
train_data = []
for raw_frame in data:
hip_pose = raw_frame[0:3].tolist()
hip_euler = raw_frame[3:6]
hip_quaternion = euler.euler2quat(
hip_euler[2] / 180.0 * np.pi, hip_euler[1] / 180.0 * np.pi,
hip_euler[0] / 180.0 * np.pi).tolist()
frame_data = hip_pose + hip_quaternion
euler_rotations = raw_frame[6:len(raw_frame)].reshape(-1, 3)
euler_index = 0
for joint in skeleton:
if (("hip" not in joint)
and (len(skeleton[joint]["channels"]) == 3)):
euler_rotation = euler_rotations[euler_index]
quaternion = euler.euler2quat(
euler_rotation[2] / 180.0 * np.pi,
euler_rotation[1] / 180.0 * np.pi,
euler_rotation[0] / 180.0 * np.pi,
axes="syxz").tolist() #z x y
frame_data = frame_data + quaternion
euler_index = euler_index + 1
elif (("hip" not in joint)
and (len(skeleton[joint]["channels"]) == 0)):
frame_data = frame_data + [1, 0, 0, 0]
train_data = train_data + [frame_data]
train_data = np.array(train_data)
return train_data
def imu_state_update(state: np.ndarray,
input: np.ndarray,
time_interval=0.01) -> np.ndarray:
'''
state transaction method base on transform3d library.
:param state:
:param input:
:param time_interval:
:return:
'''
q = euler.euler2quat(state[6], state[7], state[8])
# r_q = euler.euler2quat(input[])
w = input[3:6] * time_interval
r_q = euler.euler2quat(w[0], w[1], w[2])
q = q * r_q
# q.normalize()
# q = q.norm()
quaternions.qmult(q, r_q)
# acc = np.dot(quaternions.quat2mat(q), input[0:3]) + np.asarray((0, 0, -9.8))
acc = quaternions.rotate_vector(input[0:3], q) + np.asarray((0, 0, -9.8))
state[0:3] = state[0:3] + state[3:6] * time_interval
state[3:6] = state[3:6] + acc * time_interval
# state[6:9] = quaternions.quat2axangle(q)
state[6:9] = euler.quat2euler(q)
return state
def test_average_two(self):
"""Averaging two different quaternions."""
quat1 = euler2quat(np.pi / 4, 0, 0)
quat2 = euler2quat(-np.pi / 4, 0, 0)
avg_quat = average_quaternions([quat1, quat2])
result = quat2euler(avg_quat)
np.testing.assert_array_almost_equal(result, [0, 0, 0])
def test_angles(self):
q_sp = quaternions.qeye()
q_state = quaternions.qeye()
result = quar_axis_error(q_sp, q_state)
self.assertEqual(result[0], 0.)
self.assertEqual(result[1], 0.)
self.assertEqual(result[2], 0.)
# Roll
q_sp = euler.euler2quat(0.1,0.0,0)
result = quar_axis_error(q_sp, q_state)
self.assertAlmostEqual(result[0], 0.1, delta=1.e-8)
self.assertAlmostEqual(result[1], 0.0, delta= 1.e-8)
self.assertAlmostEqual(result[2], 0., delta=1.e-8)
# Pitch
q_sp = euler.euler2quat(0,0.1,0)
result = quar_axis_error(q_sp, q_state)
self.assertAlmostEqual(result[0], 0.0, delta = 1.e-8)
self.assertAlmostEqual(result[1], 0.1, delta = 1.e-8)
self.assertAlmostEqual(result[2], 0.0, delta = 1.e-8)
# Yaw
q_sp = euler.euler2quat(0,0,0.1)
result = quar_axis_error(q_sp, q_state)
self.assertAlmostEqual(result[0], 0.0, delta = 1.e-8)
self.assertAlmostEqual(result[1], 0.0, delta = 1.e-8)
self.assertAlmostEqual(result[2], 0.1, delta = 1.e-8)
def get_relative_viewpoint_quat(self, q, class_idx, full=False):
symmetry = [0, 4, 2, 2, 4, 1, 4, 2, 2, 1, 0, 0, 0, 3, 0, 0, 2, 0, 3, 1, 1, 2]
rot_offsets = np.array([np.radians([0, 90, 0, 0, 90, 0, 90, 0, 0, 0, 0, 0, 0, 90, 0, 0, 0, 0, 0, 94, 90, 0]),
np.radians([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, -84, 0]),
np.radians([0, 90, 0, 0, 90, -22, 90, 28, 13, 4, 0, 0, 0, 90, 0, 0, -12, 0, 92, -5, -1, 0])])
R = quat2mat(q) # no matrix inversion here
rot_adj = euler2mat(rot_offsets[0,class_idx], rot_offsets[1,class_idx], rot_offsets[2,class_idx], axes='rxyz') # inverse taken in Matlab with SpinCalc
pose = np.degrees(mat2euler(np.linalg.inv(np.dot(R,rot_adj)), axes='rxyz'))
if pose[0] > 180:
pose[0] = pose[0] - 360
elif pose[0] < -180:
pose[0] = pose[0] + 360
if pose[1] > 180:
pose[1] = pose[1] - 360
elif pose[1] < -180:
pose[1] = pose[1] + 360
# 0 = no symmetry - angle ranges: roll = (-179.5,179.5), pitch = (-89.5,89.5)
# 1 = planar symmetry - angle ranges: roll = (0.5,179.5), pitch = (-89.5,89.5)
# 2 = 2 x planar symmetry - angle ranges: roll = (0.5,89.5), pitch = (-89.5,89.5)
# 3 = infinite symmetry - angle ranges: roll = roll = 0, pitch = (-89.5,89.5)
# 4 = infinite symmetry + planar symmetry: roll = 0, pitch = (0.5,89.5)
roll = pose[0]
pitch = pose[1]
yaw = pose[2]
if symmetry[class_idx] == 1:
if roll < 0:
roll = -roll
pitch = -pitch
yaw = yaw + 180
elif symmetry[class_idx] == 2:
if roll < 0:
roll = -roll
pitch = -pitch
yaw = yaw + 180
if roll > 90:
roll = 180 - roll
pitch = -pitch
yaw = yaw + 180
elif symmetry[class_idx] == 3:
roll = 0
elif symmetry[class_idx] == 4:
roll = 0
if pitch < 0:
pitch = -pitch
yaw = yaw + 180
if yaw > 360:
yaw = yaw - 360
elif yaw < 0:
yaw = yaw + 360
if full:
vp_quat = euler2quat(np.radians(roll), np.radians(pitch), np.radians(yaw), axes='rxyz')
else:
vp_quat = euler2quat(np.radians(roll), np.radians(pitch), 0, axes='rxyz')
return vp_quat
def test():
quaternions = []
q1 = Quaternion(euler2quat(0, 0, 170 / 180 * np.pi))
q2 = Quaternion(euler2quat(0, 0, -101 / 180 * np.pi))
q3 = Quaternion(euler2quat(0, 0, 270 / 180 * np.pi))
quaternions.append(q1)
quaternions.append(q2)
quaternions.append(q3)
alpha = [1 / 3, 1 / 3, 1 / 3]
temp = calculate_avg_quaternions(alpha, quaternions, q1)
print(quat2euler(temp.vector)[2] * 180 / np.pi)
return
def get_distance_between_two_angles_longform(angle_1, angle_2):
"""
Using the long form to find the distance between two angles in euler form
"""
q1 = euler2quat(*np.deg2rad(angle_1), axes='rzxz')
q2 = euler2quat(*np.deg2rad(angle_2), axes='rzxz')
# now assume transform of the form MODAL then Something = TOTAL
# so we want to calculate MODAL^{-1} TOTAL
q_from_mode = qmult(qinverse(q2), q1)
axis, angle = quat2axangle(q_from_mode)
return np.rad2deg(angle)
def _update_markers(self, root_pos: np.ndarray, current_facing: np.ndarray,
target_facing: np.ndarray):
"""Updates the simulation markers denoting the facing direction."""
self.model.body_pos[self._markers_bid][:2] = root_pos[:2]
current_angle = np.arctan2(current_facing[1], current_facing[0])
target_angle = np.arctan2(target_facing[1], target_facing[0])
self.model.body_quat[self._current_angle_bid] = euler2quat(
0, 0, current_angle, axes='rxyz')
self.model.body_quat[self._target_angle_bid] = euler2quat(0,
0,
target_angle,
axes='rxyz')
self.sim.forward()
def do_origin(self, args):
"""Sets the given device number as the origin."""
components = args.strip().split()
if not components or not components[0]:
print('Must provide device number.')
return
device_id = components[0]
position_offset = None
if len(components) >= 4:
position_offset = list(map(float, components[1:4]))
rotation_offset = None
if len(components) >= 7:
rotation_offset = list(map(float, components[4:7]))
device = self.client.get_device(device_id)
print('Setting device {} as the origin.'.format(
device.get_model_name()))
pos_offsets = None
quat_offsets = None
if position_offset:
pos_offsets = {device: position_offset}
if rotation_offset:
quat_offsets = {device: euler2quat(*rotation_offset, axes='rxyz')}
self.client.update_coordinate_system(
origin_device=device,
device_position_offsets=pos_offsets,
device_rotation_offsets=quat_offsets)
def sensor_data_updated(self, carla_imu_measurement):
"""
Function to transform a received imu measurement into a ROS Imu message
:param carla_imu_measurement: carla imu measurement object
:type carla_imu_measurement: carla.IMUMeasurement
"""
imu_msg = Imu()
imu_msg.header = self.get_msg_header(
timestamp=carla_imu_measurement.timestamp)
# Carla uses a left-handed coordinate convention (X forward, Y right, Z up).
# Here, these measurements are converted to the right-handed ROS convention
# (X forward, Y left, Z up).
imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x
imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y
imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z
imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x
imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y
imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z
roll, pitch, yaw = trans.carla_rotation_to_RPY(
carla_imu_measurement.transform.rotation)
quat = euler2quat(roll, pitch, yaw)
imu_msg.orientation.w = quat[0]
imu_msg.orientation.x = quat[1]
imu_msg.orientation.y = quat[2]
imu_msg.orientation.z = quat[3]
self.imu_publisher.publish(imu_msg)
def drone_poses_from_states(self, states):
drn_pos = states[:, 0:3]
drn_rot_euler = states[:, 3:6].detach().cpu().numpy()
quats = [euler2quat(a[0], a[1], a[2]) for a in drn_rot_euler]
quats = torch.from_numpy(np.asarray(quats)).to(drn_pos.device)
pose = Pose(drn_pos, quats)
return pose
def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
goal = PoseStamped()
goal.header.frame_id = 'map'
goal.header.stamp = time_stamp
while True:
row = randint(side_buffer, costmap.shape[0] - side_buffer)
col = randint(side_buffer, costmap.shape[1] - side_buffer)
start_x = start.pose.position.x
start_y = start.pose.position.y
goal_x = col * res
goal_y = row * res
x_diff = goal_x - start_x
y_diff = goal_y - start_y
dist = math.sqrt(x_diff**2 + y_diff**2)
if costmap[row, col] < max_cost and dist > 3.0:
goal.pose.position.x = goal_x
goal.pose.position.y = goal_y
yaw = uniform(0, 1) * 2 * math.pi
quad = euler2quat(0.0, 0.0, yaw)
goal.pose.orientation.w = quad[0]
goal.pose.orientation.x = quad[1]
goal.pose.orientation.y = quad[2]
goal.pose.orientation.z = quad[3]
break
return goal
def _distance_from_fixed_angle(angle, fixed_angle):
"""
Designed to be mapped, this function finds the smallest rotation between
two rotations. It assumes a no-symmettry system.
The algorithm involves converting angles to quarternions, then finding the
appropriate misorientation. It is tested against the slower more complete
version finding the joining rotation.
"""
q_data = euler2quat(*angle, axes='rzxz')
q_fixed = euler2quat(*fixed_angle, axes='rzxz')
theta = np.arccos(2 * (np.dot(q_data, q_fixed))**2 - 1)
#https://math.stackexchange.com/questions/90081/quaternion-distance
return theta
def test_euler_axes():
# Test there and back with all axis specs
aba_perms = [(v[0], v[1], v[0]) for v in permutations('xyz', 2)]
axis_perms = list(permutations('xyz', 3)) + aba_perms
for (a, b, c), mat in zip(euler_tuples, euler_mats):
for rs, axes in product('rs', axis_perms):
ax_spec = rs + ''.join(axes)
conventioned = [EulerFuncs(ax_spec)]
if ax_spec in euler.__dict__:
conventioned.append(euler.__dict__[ax_spec])
mat = euler2mat(a, b, c, ax_spec)
a1, b1, c1 = mat2euler(mat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
quat = euler2quat(a, b, c, ax_spec)
a1, b1, c1 = quat2euler(quat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
ax, angle = euler2axangle(a, b, c, ax_spec)
a1, b1, c1 = axangle2euler(ax, angle, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
for obj in conventioned:
mat = obj.euler2mat(a, b, c)
a1, b1, c1 = obj.mat2euler(mat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
quat = obj.euler2quat(a, b, c)
a1, b1, c1 = obj.quat2euler(quat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
ax, angle = obj.euler2axangle(a, b, c)
a1, b1, c1 = obj.axangle2euler(ax, angle)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
def get_drone_pose(self):
drn_pos = self.get_pos_3d()
drn_rot_euler = self.get_rot_euler()
drn_rot_quat = euler.euler2quat(drn_rot_euler[0], drn_rot_euler[1],
drn_rot_euler[2])
pose = Pose(drn_pos, drn_rot_quat)
return pose
def reset(self):
if self.robot_ids is None:
self._load_model()
if self.config["waypoint_active"]:
self.initial_pos = self.way_pos[self.env.eps_count %
len(self.way_pos) - 1]
self.initial_orn = [
0, 0,
float(self.way_orn[self.env.eps_count % len(self.way_pos) - 1])
]
self.target_pos = self.way_target[self.env.eps_count %
len(self.way_pos) - 1]
else:
self.target_orn, self.target_pos = self.config[
"target_orn"], self.config["target_pos"]
self.initial_orn, self.initial_pos = self.config[
"initial_orn"], self.config["initial_pos"]
self.robot_body.reset_orientation(
quatToXYZW(euler2quat(*self.initial_orn), 'wxyz'))
self.robot_body.reset_position(self.initial_pos)
self.reset_random_pos()
self.robot_specific_reset()
state = self.calc_state()
return state
def toPoseWithCovariance(self):
pwc = PoseWithCovariance()
pwc.pose.position.x = self.x[0]
pwc.pose.position.y = self.x[1]
# pwc.pose.position.z = float(0)
q = euler2quat(0, 0, self.x[2])
pwc.pose.orientation.x = q[1]
pwc.pose.orientation.y = q[2]
pwc.pose.orientation.z = q[3]
pwc.pose.orientation.w = q[0]
# fill covariance
# I should make a function for this stuff someday
pwc.covariance[0] = self.P[0][0]
pwc.covariance[1] = self.P[0][1]
pwc.covariance[5] = self.P[0][2]
pwc.covariance[6] = self.P[1][0]
pwc.covariance[7] = self.P[1][1]
pwc.covariance[11] = self.P[1][2]
pwc.covariance[30] = self.P[2][0]
pwc.covariance[31] = self.P[2][1]
pwc.covariance[35] = self.P[2][2]
return pwc
def test_euler_axes():
# Test there and back with all axis specs
aba_perms = [(v[0], v[1], v[0]) for v in permutations('xyz', 2)]
axis_perms = list(permutations('xyz', 3)) + aba_perms
for (a, b, c), mat in zip(euler_tuples, euler_mats):
for rs, axes in product('rs', axis_perms):
ax_spec = rs + ''.join(axes)
conventioned = [EulerFuncs(ax_spec)]
if ax_spec in euler.__dict__:
conventioned.append(euler.__dict__[ax_spec])
mat = euler2mat(a, b, c, ax_spec)
a1, b1, c1 = mat2euler(mat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
quat = euler2quat(a, b, c, ax_spec)
a1, b1, c1 = quat2euler(quat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
ax, angle = euler2axangle(a, b, c, ax_spec)
a1, b1, c1 = axangle2euler(ax, angle, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
for obj in conventioned:
mat = obj.euler2mat(a, b, c)
a1, b1, c1 = obj.mat2euler(mat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
quat = obj.euler2quat(a, b, c)
a1, b1, c1 = obj.quat2euler(quat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
ax, angle = obj.euler2axangle(a, b, c)
a1, b1, c1 = obj.axangle2euler(ax, angle)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
def ego_pose_to_allo_pose_v2(ego_pose, rot_type="mat"):
assert rot_type in ["quat", "mat"], rot_type
if rot_type == "mat":
trans = ego_pose[:3, 3]
else:
trans = ego_pose[4:7]
dx = np.arctan2(trans[0], trans[2])
dy = np.arctan2(trans[1], trans[2])
# print(dx, dy)
euler_order = "sxyz"
if rot_type == "quat":
rot = ego_pose[:4]
quat = euler2quat(-dy, -dx, 0, axes=euler_order)
quat = qmult(quat, rot)
return np.concatenate([quat, trans], axis=0)
elif rot_type == "mat":
rot = ego_pose[:3, :3]
mat = euler2mat(-dy, -dx, 0, axes=euler_order)
mat = mat.dot(rot)
return np.hstack([mat, trans.reshape(3, 1)])
else:
raise ValueError(
"Unknown rot_type: {}, should be mat or quat".format(rot_type))
def __init__(self):
super().__init__()
self.phase = Phase.rotate_toward_box
self.init_control = True
self.tg_pose = None
self.up_right_q = euler.euler2quat(np.pi, 0, 0)
def __transform_from_pose_2d(ps):
msg = gms.Transform()
msg.translation = gms.Vector3(x=ps.x, y=ps.y, z=__HEIGHT)
euler = (0.0, 0.0, ps.theta)
qa = t3de.euler2quat(*euler)
quat = gms.Quaternion(w=qa[0], x=qa[1], y=qa[2], z=qa[3])
msg.rotation = quat
return msg
def get_quat(self):
"""
Function:
Convert self.alpha, self.beta, self.gamma into self.quat
"""
euler = np.array([self.alpha, self.beta, self.gamma]) / 180.0 * np.pi
quat = euler2quat(euler[0],euler[1],euler[2])
return quat
def __init__(self,
initial_pos,
initial_orn,
is_discrete=True,
target_pos=[1, 0, 0],
resolution=512,
mode="RGBD",
power=2.5,
env=None):
## WORKAROUND (hzyjerry): scaling building instead of agent, this is because
## pybullet doesn't yet support downscaling of MJCF objects
self.model_type = "MJCF"
self.mjcf_scaling = 0.25
WalkerBase.__init__(self,
"ant.xml",
"torso",
action_dim=8,
sensor_dim=28,
power=power,
target_pos=target_pos,
resolution=resolution,
scale=self.mjcf_scaling,
env=env)
self.is_discrete = is_discrete
self.initial_pos = initial_pos
self.initial_orn = initial_orn
self.eye_offset_orn = euler2quat(np.pi / 2, 0, np.pi / 2, axes='sxyz')
self.is_discrete = is_discrete
self.r_f = 0.1
if self.is_discrete:
self.action_space = gym.spaces.Discrete(17)
self.torque = 10
## Hip_1, Ankle_1, Hip_2, Ankle_2, Hip_3, Ankle_3, Hip_4, Ankle_4
self.action_list = [[self.r_f * self.torque, 0, 0, 0, 0, 0, 0, 0],
[0, self.r_f * self.torque, 0, 0, 0, 0, 0, 0],
[0, 0, self.r_f * self.torque, 0, 0, 0, 0, 0],
[0, 0, 0, self.r_f * self.torque, 0, 0, 0, 0],
[0, 0, 0, 0, self.r_f * self.torque, 0, 0, 0],
[0, 0, 0, 0, 0, self.r_f * self.torque, 0, 0],
[0, 0, 0, 0, 0, 0, self.r_f * self.torque, 0],
[0, 0, 0, 0, 0, 0, 0, self.r_f * self.torque],
[-self.r_f * self.torque, 0, 0, 0, 0, 0, 0, 0],
[0, -self.r_f * self.torque, 0, 0, 0, 0, 0, 0],
[0, 0, -self.r_f * self.torque, 0, 0, 0, 0, 0],
[0, 0, 0, -self.r_f * self.torque, 0, 0, 0, 0],
[0, 0, 0, 0, -self.r_f * self.torque, 0, 0, 0],
[0, 0, 0, 0, 0, -self.r_f * self.torque, 0, 0],
[0, 0, 0, 0, 0, 0, -self.r_f * self.torque, 0],
[0, 0, 0, 0, 0, 0, 0, -self.r_f * self.torque],
[0, 0, 0, 0, 0, 0, 0, 0]]
'''
[[self.r_f * self.torque, 0, 0, -self.r_f * self.torque, 0, 0, 0, 0],
[0, 0, self.r_f * self.torque, self.r_f * self.torque, 0, 0, 0, 0],
[0, 0, 0, 0, self.r_f * self.torque, self.r_f * self.torque, 0, 0],
[0, 0, 0, 0, 0, 0, self.r_f * self.torque, self.r_f * self.torque],
[0, 0, 0, 0, 0, 0, 0, 0]]
'''
self.setup_keys_to_action()
def test_euler2quat(self):
s = (N, N, 3)
eulers = uniform(-4, 4, size=s) * randint(2, size=s)
quats = euler2quat(eulers)
self.assertEqual(quats.shape, (N, N, 4))
for i in range(N):
for j in range(N):
res = euler.euler2quat(*eulers[i, j], axes="rxyz")
np.testing.assert_almost_equal(quats[i, j], res)
def compute_imu_orientation_from_world(robot_quat_in_world):
# imu orientation has roll and pitch relative to gravity vector. yaw in world frame
# get global yaw
yrp_world_frame = quat2euler(robot_quat_in_world, axes='szxy')
# remove global yaw rotation from roll and pitch
yaw_quat = euler2quat(yrp_world_frame[0], 0, 0, axes='szxy')
rp = rotate_vector((yrp_world_frame[1], yrp_world_frame[2], 0), qinverse(yaw_quat))
# save in correct order
return [rp[0], rp[1], 0], yaw_quat
def get_imu_quaternion(self):
# imu orientation has roll and pitch relative to gravity vector. yaw in world frame
_, robot_quat_in_world = self.get_robot_pose()
# change order to transform3d standard
robot_quat_in_world = (robot_quat_in_world[3], robot_quat_in_world[0],
robot_quat_in_world[1], robot_quat_in_world[2])
# get global yaw
yrp_world_frame = quat2euler(robot_quat_in_world, axes='szxy')
# remove global yaw rotation from roll and pitch
yaw_quat = euler2quat(yrp_world_frame[0], 0, 0, axes='szxy')
rp = rotate_vector((yrp_world_frame[1], yrp_world_frame[2], 0),
qinverse(yaw_quat))
# save in correct order
rpy = [rp[0], rp[1], 0]
# convert to quaternion
quat_wxyz = euler2quat(*rpy)
# change order to ros standard
return quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]
def turn_right(self, delta=0.03):
"""
Rotate robot base right without physics simulation
:param delta: delta angle to rotate the base right
"""
orn = self.robot_body.get_orientation()
new_orn = qmult((euler2quat(delta, 0, 0)), orn)
self.robot_body.set_orientation(new_orn)
def test_vectorised_euler2quat(random_eulers, axis_convention):
fast = vectorised_euler2quat(random_eulers, axis_convention)
stored_quats = np.ones((random_eulers.shape[0], 4))
for i, row in enumerate(random_eulers):
temp_quat = euler2quat(row[0], row[1], row[2], axis_convention)
for j in [0, 1, 2, 3]:
stored_quats[i, j] = temp_quat[j]
assert np.allclose(fast, stored_quats)
def set_marker_position_yaw(self, pos, yaw):
"""
Set subgoal marker position and orientation
:param pos: position
:param yaw: yaw angle
"""
quat = quatToXYZW(seq='wxyz', orn=euler.euler2quat(0, -np.pi / 2, yaw))
self.marker.set_position(pos)
self.marker_direction.set_position_orientation(pos, quat)
class Quadrotor(WalkerBase):
eye_offset_orn = euler2quat(np.pi / 2, 0, np.pi / 2, axes='sxyz')
def __init__(self, config, env=None):
self.model_type = "URDF"
self.mjcf_scaling = 1
self.config = config
self.is_discrete = config["is_discrete"]
WalkerBase.__init__(self,
"quadrotor.urdf",
"base_link",
action_dim=4,
sensor_dim=20,
power=2.5,
scale=self.mjcf_scaling,
initial_pos=config['initial_pos'],
target_pos=config["target_pos"],
resolution=config["resolution"],
env=env)
if self.is_discrete:
self.action_space = gym.spaces.Discrete(7)
self.action_list = [[1, 0, 0, 0, 0, 0], [-1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0], [0, -1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0], [0, 0, -1, 0, 0, 0],
[0, 0, 0, 0, 0, 0]]
self.setup_keys_to_action()
else:
action_high = 0.02 * np.ones([6])
self.action_space = gym.spaces.Box(-action_high, action_high)
self.foot_list = []
def apply_action(self, action):
if self.is_discrete:
realaction = self.action_list[action]
else:
realaction = action
p.setGravity(0, 0, 0)
p.resetBaseVelocity(self.robot_ids[0], realaction[:3], realaction[3:])
def robot_specific_reset(self):
WalkerBase.robot_specific_reset(self)
def setup_keys_to_action(self):
self.keys_to_action = {
(ord('s'), ): 0, ## backward
(ord('w'), ): 1, ## forward
(ord('d'), ): 2, ## turn right
(ord('a'), ): 3, ## turn left
(ord('z'), ): 4, ## turn left
(ord('x'), ): 5, ## turn left
(): 6
}