def reset(self):
"""Called at the end of each episode to reset position."""
obs = env.reset()
## self.ha.reset()
# Random goal location
rtra = env._sim.sample_navigable_point()
rrot = quaternion.from_euler_angles(np.array([0, 0, 0]))
obs = env._sim.get_observations_at(position=rtra,
rotation=rrot,
keep_agent_at_new_pose=True)
obs = env.step(1)
self.random_goal = obs['gps']
# Random starting location
rtra = env._sim.sample_navigable_point()
ang = np.random.rand() * 2 * np.pi
rrot = quaternion.from_euler_angles(np.array([0, ang, 0]))
obs = env._sim.get_observations_at(position=rtra,
rotation=rrot,
keep_agent_at_new_pose=True)
obs = env.step(1)
self.aloc, self.arot = obs['gps'], obs['compass']
## self.ha.update_internal_state(obs)
self.depth_map = obs['depth']
return self.agent_observation()
def test_make_orientation_rel_and_then_again_abs_2():
parent = Orientation()
parent.set_from_quaternion(quaternion.from_euler_angles(1.25, -2, 3.78))
obj = Orientation()
obj.set_from_quaternion(quaternion.from_euler_angles(-2.2, 4, 1.9))
rel_obj = make_orientation_rel(parent, obj)
assert obj == make_orientation_abs(parent, rel_obj)
def test_as_euler_angles():
np.random.seed(1843)
random_angles = [[np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi)]
for i in range(5000)]
for alpha, beta, gamma in random_angles:
R1 = quaternion.from_euler_angles(alpha, beta, gamma)
R2 = quaternion.from_euler_angles(*list(quaternion.as_euler_angles(R1)))
d = quaternion.rotation_intrinsic_distance(R1, R2)
assert d < 4e3*eps, ((alpha, beta, gamma), R1, R2, d) # Can't use allclose here; we don't care about rotor sign
def get_robot(urdf_file_path):
with open(urdf_file_path, 'r') as f:
rob = xmltd.parse(f.read())
base_link = rob["robot"]["link"][0]["inertial"]
base_link["mass"]["@value"] = np.float64(base_link["mass"]["@value"])
for key in base_link["inertia"].keys():
base_link["inertia"][key] = np.float64(base_link["inertia"][key])
base_link["origin"]["@xyz"] = np.float64(
base_link["origin"]["@xyz"].split(" "))
base_link["origin"]["@rpy"] = np.float64(
base_link["origin"]["@rpy"].split(" "))
q_bc = from_euler_angles(base_link["origin"]["@rpy"][2],
base_link["origin"]["@rpy"][1],
base_link["origin"]["@rpy"][0])
r_bc = as_rotation_matrix(q_bc.normalized())
pc = base_link["origin"]["@xyz"].reshape((3, 1))
# pc[2] += 0.03 # Uncomment if ideal alignment with COG needed.
base_link["origin"]["Tbc"] = np.concatenate((np.concatenate(
(r_bc, pc), axis=1), np.array([0, 0, 0, 1], ndmin=2)),
axis=0)
base_link["origin"]["Tcb"] = np.linalg.inv(base_link["origin"]["Tbc"])
for joint in rob["robot"]["joint"]:
joint["origin"]["@xyz"] = np.float64(
joint["origin"]["@xyz"].split(" "))
joint["origin"]["@rpy"] = np.float64(
joint["origin"]["@rpy"].split(" "))
q_bt = from_euler_angles(joint["origin"]["@rpy"][2],
joint["origin"]["@rpy"][1],
joint["origin"]["@rpy"][0])
r_bt = as_rotation_matrix(q_bt.normalized())
pt = joint["origin"]["@xyz"].reshape((3, 1))
# b - body
# t - thruster
# c - center of gravity
joint["Tbt"] = np.concatenate((np.concatenate(
(r_bt, pt), axis=1), np.array([0, 0, 0, 1], ndmin=2)),
axis=0)
joint["Tct"] = base_link["origin"]["Tcb"] @ joint["Tbt"]
return rob
def testMult(self):
pose1 = Pose3([
quaternion.from_euler_angles(0.0, 0.0, 0.0),
np.array([1.0, 0.0, 0.0])
])
pose2 = Pose3([
quaternion.from_euler_angles(0.0, 0.0, 180),
np.array([0.0, 0.0, 0.0])
])
pose3 = pose1 * pose2
# Multiplying by pose with no translation component should not change translation
self.assertTrue(
np.array_equal(pose3.translation, np.array([1.0, 0.0, 0.0])))
def test_make_orientation_rel_2():
parent = Orientation(0, 0, 0, 1)
child = Orientation()
child.set_from_quaternion(quaternion.from_euler_angles(
0.123, 0.345, 0.987))
assert make_orientation_rel(parent, child) == child
def main():
# rc = rlp.Ros('localhost', port=9090)
client = rlp.Ros('10.244.1.176', port=9090)
client.on_ready(lambda: print('is ROS connected: ', client.is_connected))
client.run()
# topic_o = '/odometry/filtered'
topic_o = '/odom'
ms = MobileClient(client,
lambda r: print('reached goal', r),
odom_topic=topic_o)
ms.wait_for_ready(timeout=80)
print('map_header: ', ms.map_header)
print(
'odom:', ms.position,
[math.degrees(v) for v in quaternion.as_euler_angles(ms.orientation)])
## you can set goal any time not only after call start().
ms.start() ## make goal appended to queue, executable
# ms.set_goal_relative_xy(0.5, 0, True) ## set scheduler a goal that go ahead 0.5 from robot body
# ms.set_goal_relative_xy(-0.5, 1) ## relative (x:front:-0.5, y:left:1)
ms.set_goal(np.quaternion(0, -0.4, -0.6, 0),
quaternion.from_euler_angles(0, 0, 1.0))
time.sleep(30)
# ms.set_goal_relative_xy(0.5, 0, True)
# time.sleep(30)
ms.stop()
print('finish')
client.terminate()
def get_relative_orientation(rel_vec: np.quaternion) -> np.quaternion:
## rotate angle (alpha,beta,gamma):(atan(y/z),atan(z/x),atan(x/y))
## don't change
to_angle = (math.atan2(rel_vec.y,
rel_vec.z), math.atan2(rel_vec.z, rel_vec.x),
math.atan2(rel_vec.x, rel_vec.y))
return quaternion.from_euler_angles(to_angle)
def __init__(self,
coordinates=(0, 0, 0),
euler=(0, 0, 0),
position=0,
quaternion=None):
# TODO rename coordinates into relative position
self.base_coordinates = np.asanyarray(coordinates)
self.euler = np.asanyarray(euler)
self.position = position
self.quaternion = quaternion
if quaternion == None:
self.quaternion = from_euler_angles(self.euler)
self.angular_speed = 0
# Finding orientation vectors
self.orientation_vector_x = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_FRONT))
self.orientation_vector_y = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_SIDE))
self.orientation_vector_z = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_TOP))
self.joined_servo_rotation_quaternion = as_quat_array(
np.insert((self.orientation_vector_y * math.sin(math.pi / 4)), 0,
-math.sin(math.pi / 4)))
# Calculate coordinates of a joint
self.update_joint_coordinates()
def cal_actor_pose(self, distance):
xp = 0.
yp = 0.
rxp = 0.
ryp = 0.
rq = Quaternion()
# xp = random.uniform(-2.8, 2.8)
# yp = random.uniform(3.0, 5.0)
# ang = 0
# rxp = xp + (distance * math.sin(math.radians(ang)))
# ryp = yp - (distance * math.cos(math.radians(ang)))
# q = quaternion.from_euler_angles(0,0,math.radians(ang))
# rq.x = q.x
# rq.y = q.y
# rq.z = q.z
# rq.w = q.w
while True:
xp = random.uniform(HUMAN_XMIN, HUMAN_XMAX)
yp = random.uniform(HUMAN_YMIN, HUMAN_YMAX)
ang = 0
rxp = xp + (distance * math.sin(math.radians(ang)))
ryp = yp - (distance * math.cos(math.radians(ang)))
human_ud_distance = math.hypot(rxp, ryp)
if rxp < HUMAN_XMAX and rxp > HUMAN_XMIN and ryp < HUMAN_YMAX and ryp > HUMAN_YMIN - distance and human_ud_distance > self.min_threshold_arrive:
# print human_ud_distance
q = quaternion.from_euler_angles(0, 0, math.radians(ang))
rq.x = q.x
rq.y = q.y
rq.z = q.z
rq.w = q.w
break
return xp, yp, rxp, ryp, rq
def randomizeTargetPose(self, obj_name):
EE_POS_TGT = np.asmatrix([[round(np.random.uniform(-0.62713, -0.29082), 5), round(np.random.uniform(-0.15654, 0.15925), 5), 0.72466]])
roll = 0.0
pitch = 0.0
yaw = np.random.uniform(-1.57, 1.57)
q = quat.from_euler_angles(roll, pitch, yaw)
EE_ROT_TGT = rot_matrix = quat.as_rotation_matrix(q)
self.target_orientation = EE_ROT_TGT
EE_POINTS = np.asmatrix([[0, 0, 0]])
ee_tgt = np.ndarray.flatten(get_ee_points(EE_POINTS, EE_POS_TGT, EE_ROT_TGT).T)
self.realgoal = ee_tgt
ms = ModelState()
ms.pose.position.x = EE_POS_TGT[0,0]
ms.pose.position.y = EE_POS_TGT[0,1]
ms.pose.position.z = EE_POS_TGT[0,2]
ms.pose.orientation.x = q.x
ms.pose.orientation.y = q.y
ms.pose.orientation.z = q.z
ms.pose.orientation.w = q.w
self._pub_link_state.publish( LinkState(link_name="target_link", pose=ms.pose, reference_frame="world") )
ms.model_name = obj_name
rospy.wait_for_service('gazebo/set_model_state')
try:
self.set_model_pose(ms)
except (rospy.ServiceException) as e:
print("Error setting the pose of " + obj_name)
def filter(args):
df = pd.read_csv(args.file)
df2 = None
q_init = None
if args.gt is not None:
df2 = pd.read_csv(args.gt)
#
# Handle RPY vs Quat.
if df2.as_matrix().shape[1] == 4:
q_init = quaternion.from_euler_angles(*df2.as_matrix()[0, 1:4])
else:
q_init = np.quaternion(*df2.as_matrix()[0, 1:5])
#
# Scale for the ts.
scale = 1
if args.isNS:
scale = 1e9
#
# Get the first message and construct.
msg = getIMUMsg(df, 0, scale)
fv = ComplementaryFilter.ComplementaryFilter(msg, q_init)
#
# Collect all states.
state = [(msg.ts, fv.getEuler(), fv.getState())]
#
# Drop the first reading and interate
for rowIdx in range(1,len(df)):
msg = getIMUMsg(df, rowIdx, scale)
fv.observation(msg)
state.append((msg.ts, fv.getEuler(), fv.getState()))
plot(args, state, df, df2)
print(fv.gyroBias)
def generate_random_PoseStamped(x_lim, y_lim, z_lim, r_min, r_max, frame_id):
# Generate random position and orientation
x_range = x_lim[1] - x_lim[0]
y_range = y_lim[1] - y_lim[0]
z_range = z_lim[1] - z_lim[0]
p = quat.from_euler_angles(2 * pi * np.random.rand(3))
x = x_lim[1] - x_range * np.random.rand()
y = y_lim[1] - y_range * np.random.rand()
z = z_lim[1] - z_range * np.random.rand()
position = np.array((x, y, z))
r = np.linalg.norm(position)
# Regenerate position if it's outside the boundaries given by r_min and r_max
while (r < r_min) or (r > r_max):
x = x_lim[1] - x_range * np.random.rand()
y = y_lim[1] - y_range * np.random.rand()
z = z_lim[1] - z_range * np.random.rand()
position = np.array((x, y, z))
r = np.linalg.norm(position)
# Fill message and publish
pose_msg = PoseStamped()
pose_msg.header.frame_id = frame_id
pose_msg.pose.position.x = x_lim[1] - x_range * np.random.rand()
pose_msg.pose.position.y = y_lim[1] - y_range * np.random.rand()
pose_msg.pose.position.z = z_lim[1] - z_range * np.random.rand()
pose_msg.pose.orientation.w = p.w
pose_msg.pose.orientation.x = p.x
pose_msg.pose.orientation.y = p.y
pose_msg.pose.orientation.z = p.z
return pose_msg
def emergency_response(self, action):
pub_action = Point()
# IN FUTURE CHANGE TO RELATIVE
if abs(action.y - self.pose_y) < 0.05:
pub_action.x = 5
pub_action.y = 0
pub_action.z = 1.5
else:
pub_action = action
wpt = MultiDOFJointTrajectory()
header = Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
wpt.joint_names.append('base_link')
wpt.header = header
quat = quaternion.from_euler_angles(0, 0, math.radians(-45))
#
# if self.active == 'astar':
# transforms = Transform(translation=self.astar, rotation=quat)
# elif self.active == 'dnn':
# transforms = Transform(translation=self.dnn, rotation=quat)
# else:
# print('No Choice Made!')
# transforms = Transform()
print('Selected {}'.format(self.active))
transforms = Transform(translation=pub_action, rotation=quat)
velocities = Twist()
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations], rospy.Time(0))
wpt.points.append(point)
self.waypoint_pub.publish(wpt)
def _generate_goal(self):
original_obj_pos, original_obj_quat = self._p.getBasePositionAndOrientation(
self.object_bodies[self.goal_object_key])
while True:
target_xyz = self.np_random.uniform(self.robot.target_bound_lower,
self.robot.target_bound_upper)
# on the table
target_xyz[-1] = self.object_initial_pos[self.goal_object_key][2]
if np.linalg.norm(target_xyz - original_obj_pos) > 0.06:
break
target_obj_euler = quat.as_euler_angles(
quat.as_quat_array([1., 0., 0., 0.]))
target_obj_euler[-1] = self.np_random.uniform(-1.0, 1.0) * np.pi
target_obj_quat = quat.as_float_array(
quat.from_euler_angles(target_obj_euler))
target_obj_quat = np.concatenate(
[target_obj_quat[1:], [target_obj_quat[0]]], axis=-1)
self.desired_goal = np.concatenate((target_xyz, target_obj_euler))
if self.visualize_target:
self.set_object_pose(
self.object_bodies[self.goal_object_key + '_target'],
target_xyz, target_obj_quat)
def test_Wigner_D_element_values(special_angles, ell_max):
LMpM = sf.LMpM_range_half_integer(0, ell_max // 2)
# Compare with more explicit forms given in Euler angles
print("")
for alpha in special_angles:
print("\talpha={0}".format(
alpha)) # Need to show some progress to Travis
for beta in special_angles:
print("\t\tbeta={0}".format(beta))
for gamma in special_angles:
a = np.conjugate(
np.array([
slow_Wigner_D_element(alpha, beta, gamma, ell, mp, m)
for ell, mp, m in LMpM
]))
b = sf.Wigner_D_element(
quaternion.from_euler_angles(alpha, beta, gamma), LMpM)
# if not np.allclose(a, b,
# atol=ell_max ** 6 * precision_Wigner_D_element,
# rtol=ell_max ** 6 * precision_Wigner_D_element):
# for i in range(min(a.shape[0], 100)):
# print(LMpM[i], "\t", abs(a[i]-b[i]), "\t\t", a[i], "\t", b[i])
assert np.allclose(
a,
b,
atol=ell_max**6 * precision_Wigner_D_element,
rtol=ell_max**6 * precision_Wigner_D_element)
def test_error(self):
translation = [144.801, -74, -20]
rotation = quaternion.from_euler_angles(np.deg2rad(110.0), 0, 0)
translation_gt = [144, -74, -20]
rotation_gt = quaternion.from_euler_angles(np.deg2rad(125.0), 0, 0)
pose_a = kapture.PoseTransform(r=rotation, t=translation).inverse()
pose_a_gt = kapture.PoseTransform(r=rotation_gt,
t=translation_gt).inverse()
pose_b = kapture.PoseTransform(r=None, t=[-x for x in translation])
pose_b_gt = kapture.PoseTransform(r=None,
t=[-x for x in translation_gt])
pose_c = kapture.PoseTransform(r=rotation.inverse(), t=None)
pose_c_gt = kapture.PoseTransform(r=rotation_gt.inverse(), t=None)
pose_d = kapture.PoseTransform(r=None, t=None)
pose_d_gt = kapture.PoseTransform(r=None, t=None)
poses = [('a', pose_a), ('b', pose_b), ('c', pose_c), ('d', pose_d),
('e', pose_d)]
poses_gt = [('a', pose_a_gt), ('b', pose_b_gt), ('c', pose_c_gt),
('d', pose_d_gt), ('e', pose_a_gt)]
error = evaluate_error_absolute(poses, poses_gt)
self.assertEqual(error[0][0], 'a')
self.assertAlmostEqual(error[0][1], 0.801)
self.assertAlmostEqual(error[0][2], 15.0)
self.assertEqual(error[1][0], 'b')
self.assertAlmostEqual(error[1][1], 0.801)
self.assertTrue(math.isnan(error[1][2]))
self.assertEqual(error[2][0], 'c')
self.assertTrue(math.isnan(error[2][1]))
self.assertAlmostEqual(error[2][2], 15.0)
self.assertEqual(error[3][0], 'd')
self.assertTrue(math.isnan(error[3][1]))
self.assertTrue(math.isnan(error[3][2]))
self.assertEqual(error[4][0], 'e')
self.assertTrue(math.isnan(error[4][1]))
self.assertTrue(math.isnan(error[4][2]))
def sample_quat(angle3):
"""Sample a quaterion from a range of euler angles in degrees"""
roll = sample(angle3[0]) * np.pi / 180
pitch = sample(angle3[1]) * np.pi / 180
yaw = sample(angle3[2]) * np.pi / 180
quat = quaternion.from_euler_angles(roll, pitch, yaw)
return quat.normalized().components
def testInit(self):
pose = Pose3([
quaternion.from_euler_angles(0.0, 0.0, 0.0),
np.array([1.0, 0.0, 0.0])
])
self.assertEqual(pose.rotation,
quaternion.from_float_array([1, 0, 0, 0]))
self.assertEqual(np.linalg.norm(pose.translation), 1.0)
def euler2quaternion(att):
# attitude change
# att = ([
# gamma, psi (yaw) <- ra
# beta, theta (pitch) <- de
# alpha, phi (roll) <- roll
# ])
# TODO: really f*****g check this with D'Amico's papers
return quaternion.from_euler_angles(att[2], att[1], att[0])
def test_SWSH_values(special_angles, ell_max):
for iota in special_angles:
for phi in special_angles:
for ell in range(ell_max + 1):
for s in range(-ell, ell + 1):
for m in range(-ell, ell + 1):
R = quaternion.from_euler_angles(phi, iota, 0)
assert abs(sf.SWSH(R, s, np.array([[ell, m]]))
- slow_sYlm(s, ell, m, iota, phi)) < ell_max ** 6 * precision_SWSH
def get_extrinsic_parameter(self):
trans = np.zeros((3, 1)) # translation is 0, 0, 0
v_lidar = np.array([-1.57079633, 3.12042851, -1.57079633])
v_cam = np.array([-3.13498819, 1.59196951, 1.56942932])
v_diff = v_cam - v_lidar
q = quaternion.from_euler_angles(v_diff)
R_ = quaternion.as_rotation_matrix(q)
RT = np.concatenate((R_, trans), axis=-1)
return RT
def jitter_quaternions(original_quaternion, rnd, angle=30.0):
original_euler = quaternion.as_euler_angles(original_quaternion)
euler_angles = np.array([
(rnd.rand() - 0.5) * np.pi * angle / 180.0 + original_euler[0],
(rnd.rand() - 0.5) * np.pi * angle / 180.0 + original_euler[1],
(rnd.rand() - 0.5) * np.pi * angle / 180.0 + original_euler[2],
])
quaternions = quaternion.from_euler_angles(euler_angles)
return quaternions
def testInverse(self):
pose = Pose3([
quaternion.from_euler_angles(0.0, 0.0, 0.0),
np.array([1.0, 0.0, 0.0])
])
self.assertEqual(pose.inverse().rotation,
quaternion.from_float_array([1, 0, 0, 0]))
self.assertTrue(
np.array_equal(pose.inverse().translation,
np.array([-1.0, 0.0, 0.0])))
def check_box_collision(self, joints, box):
'''
Arguments: joints represents the current location of the robot
box contains the position of the center of the box [x, y, z, r, p, y] and the length, width, and height [l, w, h]
Returns: A boolean where True means the box is in collision with the arm and false means that there are no collisions.
'''
box_pos, box_rpy, box_hsizes = box[:3], box[3:6], box[6:] / 2
box_q = quaternion.from_euler_angles(box_rpy)
box_axes = quaternion.as_rotation_matrix(box_q)
self._box_vertices_offset[:, :] = self._vertex_offset_signs * box_hsizes
box_vertices = (box_axes @ self._box_vertices_offset.T +
np.expand_dims(box_pos, 1)).T
box_hdiag = np.linalg.norm(box_hsizes)
min_col_dists = box_hdiag + self._collision_box_hdiags
franka_box_poses = self.get_collision_boxes_poses(joints)
for i, franka_box_pose in enumerate(franka_box_poses):
fbox_pos = franka_box_pose[:3, 3]
fbox_axes = franka_box_pose[:3, :3]
# coarse collision check
if np.linalg.norm(fbox_pos - box_pos) > min_col_dists[i]:
continue
fbox_vertex_offsets = self._collision_box_vertices_offset[i]
fbox_vertices = fbox_vertex_offsets @ fbox_axes.T + fbox_pos
# fbox_vertices = np.matmul(fbox_vertex_offsets,fbox_axes.T) + fbox_pos
# construct axes
cross_product_pairs = np.array(
list(product(box_axes.T, fbox_axes.T)))
cross_axes = np.cross(cross_product_pairs[:, 0],
cross_product_pairs[:, 1]).T
self._collision_proj_axes[:, :3] = box_axes
self._collision_proj_axes[:, 3:6] = fbox_axes
self._collision_proj_axes[:, 6:] = cross_axes
# projection
box_projs = box_vertices @ self._collision_proj_axes
fbox_projs = fbox_vertices @ self._collision_proj_axes
min_box_projs, max_box_projs = box_projs.min(
axis=0), box_projs.max(axis=0)
min_fbox_projs, max_fbox_projs = fbox_projs.min(
axis=0), fbox_projs.max(axis=0)
# check if no separating planes exist
if np.all([
min_box_projs <= max_fbox_projs,
max_box_projs >= min_fbox_projs
]):
return True
return False
def randomizeTargetPose(self, obj_name, centerPoint=None):
ms = ModelState()
if centerPoint is None:
EE_POS_TGT = np.asmatrix([
round(np.random.uniform(-0.62713, -0.29082), 5),
round(np.random.uniform(-0.15654, 0.15925), 5),
self.realgoal[2]
])
roll = 0.0
pitch = 0.0
yaw = np.random.uniform(-1.57, 1.57)
q = quat.from_euler_angles(roll, pitch, yaw)
EE_ROT_TGT = rot_matrix = quat.as_rotation_matrix(q)
self.target_orientation = EE_ROT_TGT
ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'],
EE_POS_TGT, EE_ROT_TGT).T)
ms.pose.position.x = EE_POS_TGT[0, 0]
ms.pose.position.y = EE_POS_TGT[0, 1]
ms.pose.position.z = EE_POS_TGT[0, 2]
ms.pose.orientation.x = q.x
ms.pose.orientation.y = q.y
ms.pose.orientation.z = q.z
ms.pose.orientation.w = q.w
ms.model_name = obj_name
rospy.wait_for_service('gazebo/set_model_state')
try:
self.set_model_pose(ms)
except (rospy.ServiceException) as e:
print("Error setting the pose of " + obj_name)
else:
EE_POS_TGT = np.asmatrix(
[self.realgoal[0], self.realgoal[1], centerPoint])
ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'],
EE_POS_TGT, self.target_orientation).T)
ms.pose.position.x = EE_POS_TGT[0, 0]
ms.pose.position.y = EE_POS_TGT[0, 1]
ms.pose.position.z = EE_POS_TGT[0, 2]
ms.pose.orientation.x = 0
ms.pose.orientation.y = 0
ms.pose.orientation.z = 0
ms.pose.orientation.w = 0
self._pub_link_state.publish(
LinkState(link_name="target_link",
pose=ms.pose,
reference_frame="world"))
self.realgoal = ee_tgt
def _task_reset(self, test=False):
if not self.objects_urdf_loaded:
# don't reload object urdf
self.objects_urdf_loaded = True
self.object_bodies['workspace'] = self._p.loadURDF(
os.path.join(self.object_assets_path, "workspace.urdf"),
basePosition=self.object_initial_pos['workspace'][:3],
baseOrientation=self.object_initial_pos['workspace'][3:])
for object_key in self.manipulated_object_keys:
self.object_bodies[object_key] = self._p.loadURDF(
os.path.join(self.object_assets_path, object_key+".urdf"),
basePosition=self.object_initial_pos[object_key][:3],
baseOrientation=self.object_initial_pos[object_key][3:])
self.object_bodies[self.goal_object_key+'_target'] = self._p.loadURDF(
os.path.join(self.object_assets_path, self.goal_object_key+"_target.urdf"),
basePosition=self.object_initial_pos['target'][:3],
baseOrientation=self.object_initial_pos['target'][3:])
if not self.visualize_target:
self.set_object_pose(self.object_bodies[self.goal_object_key+'_target'],
[0.0, 0.0, -3.0],
self.object_initial_pos['target'][3:])
# randomize object positions
object_poses = []
object_quats = []
for object_key in self.manipulated_object_keys:
done = False
while not done:
new_object_xy = self.np_random.uniform(self.robot.object_bound_lower[:-1],
self.robot.object_bound_upper[:-1])
object_not_overlap = []
for pos in object_poses + [self.robot.end_effector_tip_initial_position]:
object_not_overlap.append((np.linalg.norm(new_object_xy - pos[:-1]) > 0.06))
if all(object_not_overlap):
object_poses.append(np.concatenate((new_object_xy.copy(), [self.object_initial_pos[object_key][2]])))
done = True
orientation_euler = quat.as_euler_angles(quat.as_quat_array([1., 0., 0., 0.]))
orientation_euler[-1] = self.np_random.uniform(-1.0, 1.0) * np.pi
orientation_quat_new = quat.as_float_array(quat.from_euler_angles(orientation_euler))
orientation_quat_new = np.concatenate([orientation_quat_new[1:], [orientation_quat_new[0]]], axis=-1)
object_quats.append(orientation_quat_new.copy())
self.set_object_pose(self.object_bodies[object_key],
object_poses[-1],
orientation_quat_new)
# generate goals & images
self._generate_goal()
if self.goal_image:
self._generate_goal_image()
def test_SWSH_WignerD_expression(special_angles, ell_max):
for iota in special_angles:
for phi in special_angles:
for ell in range(ell_max + 1):
for s in range(-ell, ell + 1):
R = quaternion.from_euler_angles(phi, iota, 0)
LM = np.array([[ell, m] for m in range(-ell, ell + 1)])
Y = sf.SWSH(R, s, LM)
LMS = np.array([[ell, m, -s] for m in range(-ell, ell + 1)])
D = (-1.) ** (s) * math.sqrt((2 * ell + 1) / (4 * np.pi)) * sf.Wigner_D_element(R.a, R.b, LMS)
assert np.allclose(Y, D, atol=ell ** 6 * precision_SWSH, rtol=ell ** 6 * precision_SWSH)
def test_from_euler_angles():
np.random.seed(1843)
random_angles = [[np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi)]
for i in range(5000)]
for alpha, beta, gamma in random_angles:
assert abs((np.quaternion(0, 0, 0, alpha / 2.).exp()
* np.quaternion(0, 0, beta / 2., 0).exp()
* np.quaternion(0, 0, 0, gamma / 2.).exp()
)
- quaternion.from_euler_angles(alpha, beta, gamma)) < 1.e-15
def __init__(self,
origin=Vector3(),
orientation=3 * (Angle(0, units.rad), ),
name='world'):
self.origin = origin # * dimension_unit
if not all(isinstance(i, Angle) for i in orientation):
raise TypeError('Expected input transformation to be a list/tuple '
'of length 3 with all elements of type Angle.')
euler_angles = [i.to(units.rad).value for i in orientation]
self.orientation = quaternion.from_euler_angles(euler_angles)
self.name = name
def xy_yaw_to_agent_state(self, x, y, yaw):
# xy: spot has x point forward, y point left; but habitat agent state has x point right, z point back
position = np.array([-y, 0., -x], dtype=np.float32)
# yaw: magic conversion... debugged by taking habitat agent_state().rotation and reproduce it from yaw
# it is roughly the inverse of this function
# https://github.com/facebookresearch/habitat-lab/blob/b7a93bc493f7fb89e5bf30b40be204ff7b5570d7/habitat/tasks/nav/nav.py#L864
# quaternion.from_euler_angles(np.pi, -true_yaw - np.pi/2, np.pi)
rotation = quaternion.from_euler_angles(np.pi, -yaw,
np.pi) # - np.pi/2
return AgentState(position=position, rotation=rotation)
def test_SWSH_grid(special_angles, ell_max):
LM = sf.LM_range(0, ell_max)
# Test flat array arrangement
R_grid = np.array([quaternion.from_euler_angles(alpha, beta, gamma).normalized()
for alpha in special_angles
for beta in special_angles
for gamma in special_angles])
for s in range(-ell_max + 1, ell_max):
values_explicit = np.array([sf.SWSH(R, s, LM) for R in R_grid])
values_grid = sf.SWSH_grid(R_grid, s, ell_max)
assert np.array_equal(values_explicit, values_grid)
# Test nested array arrangement
R_grid = np.array([[[quaternion.from_euler_angles(alpha, beta, gamma)
for alpha in special_angles]
for beta in special_angles]
for gamma in special_angles])
for s in range(-ell_max + 1, ell_max):
values_explicit = np.array([[[sf.SWSH(R, s, LM) for R in R1] for R1 in R2] for R2 in R_grid])
values_grid = sf.SWSH_grid(R_grid, s, ell_max)
assert np.array_equal(values_explicit, values_grid)
def test_Wigner_D_input_types(Rs, special_angles, ell_max):
LMpM = sf.LMpM_range(0, ell_max // 2)
# Compare with more explicit forms given in Euler angles
print("")
for alpha in special_angles:
print("\talpha={0}".format(alpha)) # Need to show some progress to Travis
for beta in special_angles:
for gamma in special_angles:
a = sf.Wigner_D_element(alpha, beta, gamma, LMpM)
b = sf.Wigner_D_element(quaternion.from_euler_angles(alpha, beta, gamma), LMpM)
assert np.allclose(a, b,
atol=ell_max ** 6 * precision_Wigner_D_element,
rtol=ell_max ** 6 * precision_Wigner_D_element)
for R in Rs:
a = sf.Wigner_D_element(R, LMpM)
b = sf.Wigner_D_element(R.a, R.b, LMpM)
assert np.allclose(a, b,
atol=ell_max ** 6 * precision_Wigner_D_element,
rtol=ell_max ** 6 * precision_Wigner_D_element)
def test_Wigner_D_element_values(special_angles, ell_max):
LMpM = sf.LMpM_range_half_integer(0, ell_max // 2)
# Compare with more explicit forms given in Euler angles
print("")
for alpha in special_angles:
print("\talpha={0}".format(alpha)) # Need to show some progress to Travis
for beta in special_angles:
print("\t\tbeta={0}".format(beta))
for gamma in special_angles:
a = np.conjugate(np.array([slow_Wigner_D_element(alpha, beta, gamma, ell, mp, m) for ell,mp,m in LMpM]))
b = sf.Wigner_D_element(quaternion.from_euler_angles(alpha, beta, gamma), LMpM)
# if not np.allclose(a, b,
# atol=ell_max ** 6 * precision_Wigner_D_element,
# rtol=ell_max ** 6 * precision_Wigner_D_element):
# for i in range(min(a.shape[0], 100)):
# print(LMpM[i], "\t", abs(a[i]-b[i]), "\t\t", a[i], "\t", b[i])
assert np.allclose(a, b,
atol=ell_max ** 6 * precision_Wigner_D_element,
rtol=ell_max ** 6 * precision_Wigner_D_element)
def _get_orientation_ekf(self, Ca):
"""Extended Kalman filter for sensor fusion
x is the state: [theta, bias, adyn]^T
where theta are the Euler """
if self.Qbias is None or self.Qgyro is None or self.Qacc is None:
raise ValueError('Noise covariance is not yet estimated')
if self.gN is None:
raise ValueError('Inertial reference frame is not yet set')
self.Qdyn = np.diag(Ca).dot(self.Qacc)
xkm1 = np.zeros((9,))
dt = np.diff(self.t)
dt = np.insert(dt, 0, dt[0:0])
dt1 = dt[0]
# make 9 x 9 matrix
Pkm1 = self._stack_matrices([
[(self.Qgyro + self.Qbias)*dt1**2, -self.Qbias*dt1, np.zeros((3, 3))],
[-self.Qbias*dt1, self.Qbias, np.zeros((3, 3))],
[np.zeros((3, 3)), np.zeros((3, 3)), self.Qdyn]])
N = self.gyro.shape[0]
gyro = np.deg2rad(self.gyro) - self.bias_gyro
acc = 9.81*self.acc
eulerEKF = []
aD = []
err = []
PkEKF = []
xkEKF = []
Rk = self.Qacc
for dt1, omegak, accel in zip(dt, gyro, acc):
Fk, xkM, Bk = self._system_dynamics(xkm1, omegak, dt1, Ca)
Qk = self._stack_matrices([
[Bk.dot(self.Qgyro + self.Qbias).dot(Bk.T)*dt1**2, -Bk.dot(self.Qbias)*dt1, np.zeros((3,3))],
[-self.Qbias.dot(Bk.T)*dt1, self.Qbias, np.zeros((3,3))],
[np.zeros((3,3)), np.zeros((3,3)), self.Qdyn]])
PkM = Fk.dot(Pkm1).dot(Fk.T) + Qk
hk, Jh = self._observation_dynamics(xkM, self.gN)
Hk = Jh
Sk = Hk.dot(PkM).dot(Hk.T) + Rk
Kk = PkM.dot(Hk.T).dot(np.linalg.pinv(Sk))
xk = xkM + Kk.dot(accel - hk)
Pk = (np.eye(9) - Kk.dot(Hk)).dot(PkM)
QT = self._getQT(xk[:3])
eulerEKF.append(xk[:3])
aD.append(QT.T.dot(xk[6:]))
err.append(accel - ((QT.dot(self.gN) + xk[6:])))
PkEKF.append(Pk)
xkEKF.append(xk)
Pkm1 = Pk
xkm1 = xk
self.orient_sensor = np.pad(np.array(eulerEKF), ((1, 0), (0, 0)), mode='edge')
self.accdyn_sensor = np.pad(np.array(aD), ((1, 0), (0, 0)), mode='edge')
qorient = []
for o1 in self.orient_sensor:
qorient.append(quaternion.from_euler_angles(*o1))
self.qorient = np.array(qorient)
