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 allocentric_to_egocentric(allo_pose, src_type="mat", dst_type="mat"):
"""Given an allocentric (object-centric) pose, compute new camera-centric
pose Since we do detection on the image plane and our kernels are
2D-translationally invariant, we need to ensure that rendered objects
always look identical, independent of where we render them.
Since objects further away from the optical center undergo skewing,
we try to visually correct by rotating back the amount between
optical center ray and object centroid ray. Another way to solve
that might be translational variance
(https://arxiv.org/abs/1807.03247)
"""
# Compute rotation between ray to object centroid and optical center ray
cam_ray = np.asarray([0, 0, 1.0])
if src_type == "mat":
trans = allo_pose[:3, 3]
elif src_type == "quat":
trans = allo_pose[4:7]
else:
raise ValueError(
"src_type should be mat or quat, got: {}".format(src_type))
obj_ray = trans.copy() / np.linalg.norm(trans)
angle = math.acos(cam_ray.dot(obj_ray))
# Rotate back by that amount
if angle > 0:
if dst_type == "mat":
ego_pose = np.zeros((3, 4), dtype=allo_pose.dtype)
ego_pose[:3, 3] = trans
rot_mat = axangle2mat(axis=np.cross(cam_ray, obj_ray), angle=angle)
if src_type == "mat":
ego_pose[:3, :3] = np.dot(rot_mat, allo_pose[:3, :3])
elif src_type == "quat":
ego_pose[:3, :3] = np.dot(rot_mat, quat2mat(allo_pose[:4]))
elif dst_type == "quat":
ego_pose = np.zeros((7, ), dtype=allo_pose.dtype)
ego_pose[4:7] = trans
rot_q = axangle2quat(np.cross(cam_ray, obj_ray), angle)
if src_type == "quat":
ego_pose[:4] = qmult(rot_q, allo_pose[:4])
elif src_type == "mat":
ego_pose[:4] = qmult(rot_q, mat2quat(allo_pose[:3, :3]))
else:
raise ValueError(
"dst_type should be mat or quat, got: {}".format(dst_type))
else: # allo to ego
if src_type == "mat" and dst_type == "quat":
ego_pose = np.zeros((7, ), dtype=allo_pose.dtype)
ego_pose[:4] = mat2quat(allo_pose[:3, :3])
ego_pose[4:7] = allo_pose[:3, 3]
elif src_type == "quat" and dst_type == "mat":
ego_pose = np.zeros((3, 4), dtype=allo_pose.dtype)
ego_pose[:3, :3] = quat2mat(allo_pose[:4])
ego_pose[:3, 3] = allo_pose[4:7]
else:
ego_pose = allo_pose.copy()
return ego_pose
def build_camreas(self):
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_front = b.build(True, name="camera")
pitch = np.deg2rad(-15)
cam_front.set_pose(
Pose([1, 0, 1],
qmult(axangle2quat([0, 0, 1], np.pi),
axangle2quat([0, -1, 0], pitch))))
c = self.scene.add_mounted_camera('front_camera', cam_front, Pose(),
640, 480, 0, np.deg2rad(45), 0.1, 10)
self.front_camera = Camera(c)
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1]))
b.build(True, name="tripod").set_pose(Pose([1, 0, 1]))
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_left = b.build(True, name="camera")
pitch = np.deg2rad(-15)
cam_left.set_pose(
Pose([0, 1.5, 1],
qmult(axangle2quat([0, 0, 1], -np.pi / 2),
axangle2quat([0, -1, 0], pitch))))
c = self.scene.add_mounted_camera('left_camera', cam_left, Pose(), 640,
480, 0, np.deg2rad(45), 0.1, 10)
self.left_camera = Camera(c)
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1]))
b.build(True, name="tripod").set_pose(Pose([0, 1.5, 1]))
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_right = b.build(True, name="camera")
pitch = np.deg2rad(-15)
cam_right.set_pose(
Pose([0, -1.5, 1],
qmult(axangle2quat([0, 0, 1], np.pi / 2),
axangle2quat([0, -1, 0], pitch))))
c = self.scene.add_mounted_camera('right_camera', cam_right, Pose(),
640, 480, 0, np.deg2rad(45), 0.1, 10)
self.right_camera = Camera(c)
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/tripod.dae', Pose([0, 0, -1]))
b.build(True, name="tripod").set_pose(Pose([0, -1.5, 1]))
b = self.scene.create_actor_builder()
b.add_visual_from_file('./assets/camera.dae')
cam_top = b.build(True, name="camera")
pitch = np.deg2rad(-90)
cam_top.set_pose(Pose([-0.5, 0, 3], axangle2quat([0, -1, 0], pitch)))
c = self.scene.add_mounted_camera('top_camera', cam_top, Pose(), 640,
480, 0, np.deg2rad(45), 0.1, 10)
self.top_camera = Camera(c)
def findQuaternionMean(sigmas):
errors = np.zeros((6, 3))
oldMean = sigmas[0, :]
for i in range(1, len(sigmas)):
currentQ = sigmas[i, :]
errorQ = quat.qmult(currentQ, oldMean)
vectorError = quatToAxisAngle(errorQ)
errors[i, :] = vectorError
averageErrorVector = np.mean(errors, axis=0)
quatError = axisAngleToQuat(averageErrorVector)
newMean = quat.qmult(quatError, oldMean)
return newMean, errors, averageErrorVector
def egocentric_to_allocentric(ego_pose,
src_type="mat",
dst_type="mat",
cam_ray=(0, 0, 1.0)):
# Compute rotation between ray to object centroid and optical center ray
cam_ray = np.asarray(cam_ray)
if src_type == "mat":
trans = ego_pose[:3, 3]
elif src_type == "quat":
trans = ego_pose[4:7]
else:
raise ValueError(
"src_type should be mat or quat, got: {}".format(src_type))
obj_ray = trans.copy() / np.linalg.norm(trans)
angle = math.acos(cam_ray.dot(obj_ray))
# Rotate back by that amount
if angle > 0:
if dst_type == "mat":
allo_pose = np.zeros((3, 4), dtype=ego_pose.dtype)
allo_pose[:3, 3] = trans
rot_mat = axangle2mat(axis=np.cross(cam_ray, obj_ray),
angle=-angle)
if src_type == "mat":
allo_pose[:3, :3] = np.dot(rot_mat, ego_pose[:3, :3])
elif src_type == "quat":
allo_pose[:3, :3] = np.dot(rot_mat, quat2mat(ego_pose[:4]))
elif dst_type == "quat":
allo_pose = np.zeros((7, ), dtype=ego_pose.dtype)
allo_pose[4:7] = trans
rot_q = axangle2quat(np.cross(cam_ray, obj_ray), -angle)
if src_type == "quat":
allo_pose[:4] = qmult(rot_q, ego_pose[:4])
elif src_type == "mat":
allo_pose[:4] = qmult(rot_q, mat2quat(ego_pose[:3, :3]))
else:
raise ValueError(
"dst_type should be mat or quat, got: {}".format(dst_type))
else:
if src_type == "mat" and dst_type == "quat":
allo_pose = np.zeros((7, ), dtype=ego_pose.dtype)
allo_pose[:4] = mat2quat(ego_pose[:3, :3])
allo_pose[4:7] = ego_pose[:3, 3]
elif src_type == "quat" and dst_type == "mat":
allo_pose = np.zeros((3, 4), dtype=ego_pose.dtype)
allo_pose[:3, :3] = quat2mat(ego_pose[:4])
allo_pose[:3, 3] = ego_pose[4:7]
else:
allo_pose = ego_pose.copy()
return allo_pose
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 reset_random_pos(self):
'''Add randomness to resetted initial position
'''
if not self.config["random"]["random_initial_pose"]:
return
pos = self.robot_body.get_position()
orn = self.robot_body.get_orientation()
x_range = self.config["random"]["random_init_x_range"]
y_range = self.config["random"]["random_init_y_range"]
z_range = self.config["random"]["random_init_z_range"]
r_range = self.config["random"]["random_init_rot_range"]
new_pos = [
pos[0] + self.np_random.uniform(low=x_range[0], high=x_range[1]),
pos[1] + self.np_random.uniform(low=y_range[0], high=y_range[1]),
pos[2] + self.np_random.uniform(low=z_range[0], high=z_range[1])
]
new_orn = quaternions.qmult(
quaternions.axangle2quat([1, 0, 0],
self.np_random.uniform(low=r_range[0],
high=r_range[1])),
orn)
self.robot_body.reset_orientation(new_orn)
self.robot_body.reset_position(new_pos)
def reset_random_target(self):
'''
Reposition target randomness
'''
if not self.config["random"]["random_target_pose"]:
return
pos = self.config["target_pos"]
orn = self.config["target_orn"]
x_range = self.config["random"]["random_init_x_range"]
y_range = self.config["random"]["random_init_y_range"]
z_range = self.config["random"]["random_init_z_range"]
r_range = self.config["random"]["random_init_rot_range"]
new_pos = [
pos[0] + self.np_random.uniform(low=x_range[0], high=x_range[1]),
pos[1] + self.np_random.uniform(low=y_range[0], high=y_range[1]),
pos[2] + self.np_random.uniform(low=z_range[0], high=z_range[1])
]
new_orn = quaternions.qmult(
quaternions.axangle2quat([1, 0, 0],
self.np_random.uniform(low=r_range[0],
high=r_range[1])),
orn)
self.config["target_pos"] = new_pos
self.config["target_orn"] = new_orn
def reset_base_position(self, enabled, delta_orn=np.pi / 9, delta_pos=0.2):
#print("Reset base enabled", enabled)
if not enabled:
return
pos = self.robot_body.current_position()
orn = self.robot_body.current_orientation()
#print("collision", len(p.getContactPoints(self.robot_body.bodyIndex)))
#while True:
new_pos = [
pos[0] + self.np_random.uniform(
low=self.env.config["random_init_x_range"][0],
high=self.env.config["random_init_x_range"][1]),
pos[1] + self.np_random.uniform(
low=self.env.config["random_init_y_range"][0],
high=self.env.config["random_init_y_range"][1]),
pos[2] + self.np_random.uniform(
low=self.env.config["random_init_z_range"][0],
high=self.env.config["random_init_z_range"][1])
]
new_orn = quat.qmult(
quat.axangle2quat(
[1, 0, 0],
self.np_random.uniform(
low=self.env.config["random_init_rot_range"][0],
high=self.env.config["random_init_rot_range"][1])), orn)
self.robot_body.reset_orientation(new_orn)
self.robot_body.reset_position(new_pos)
def quater2ang_v(quater_1, quater_2, dt):
delta_q = qmult(quater_2, qconjugate(quater_1))
delta_q_len = np.linalg.norm(delta_q[1:])
delta_q_angle = 2 * np.arctan2(delta_q_len, delta_q[0])
w = delta_q[1:] * delta_q_angle * 1 / dt
return w
def getRelativeQuaternionAtoB(quat_a, quat_b):
"""
Given two quaternions A and B, determines the quaternion that rotates A to B
(Multiplying A by q_rel will give B)
"""
q_rel = qmult(qinverse(quat_a), quat_b)
return q_rel
def mocap_set_action(sim, action):
"""The action controls the robot using mocaps. Specifically, bodies
on the robot (for example the gripper wrist) is controlled with
mocap bodies. In this case the action is the desired difference
in position and orientation (quaternion), in world coordinates,
of the of the target body. The mocap is positioned relative to
the target body according to the delta, and the MuJoCo equality
constraint optimizer tries to center the welded body on the mocap.
"""
if sim.model.nmocap > 0:
action, _ = np.split(action, (sim.model.nmocap * 7, ))
action = action.reshape(sim.model.nmocap, 7)
pos_delta = action[:, :3].ravel()
quat_delta = action[:, 3:].ravel()
reset_mocap2body_xpos(sim)
print("pos_delta", pos_delta.shape)
sim.data.mocap_pos[:] = sim.data.mocap_pos + pos_delta
# upright
default = np.array([1, 0, 1, 0.0])
default /= np.linalg.norm(default)
sim.data.mocap_quat[:] = quaternions.qmult(default, quat_delta)
def _set_action(self, action): assert action.shape == (self.n_actions,) action = action.copy() # ensure that we don't change the action outside of this scope if self.block_gripper: pos_ctrl = action else: pos_ctrl, gripper_ctrl = action[:3], action[3] gripper_ctrl = (gripper_ctrl+1.0)/2.0 pos_ee, quat_ee = self.psm.getPoseAtEE() pos_ee = pos_ee + pos_ctrl*0.001 # the maximum change in position is 0.1cm #Get table information to constrain orientation and position pos_table, q_table = self.table.getPose(self.psm.base_handle) #Make sure tool tip is not in the table by checking tt and which side of the table it is on #DH parameters to find tt position ct = np.cos(0) st = np.sin(0) ca = np.cos(-np.pi/2.0) sa = np.sin(-np.pi/2.0) T_x = np.array([[1, 0, 0, 0], [0, ca, -sa, 0 ], [0, sa, ca, 0 ], [0, 0, 0, 1 ]]) T_z = np.array([[ct, -st, 0, 0], [st, ct, 0, 0], [0, 0, 1, 0.0102], [0, 0, 0, 1]]) ee_T_tt = np.dot(T_x, T_z) pos_tt, quat_tt = self.psm.matrix2posquat(np.dot(self.psm.posquat2Matrix(pos_ee,quat_ee), ee_T_tt)) pos_tt_on_table, distanceFromTable = self._project_point_on_table(pos_tt) #if the distance from the table is negative, then we need to project pos_tt onto the table top. #Or if two dim only are enabled if distanceFromTable < 0 or self.two_dimension_only: pos_ee, _ = self.psm.matrix2posquat(np.dot(self.psm.posquat2Matrix(pos_tt_on_table, quat_tt), np.linalg.inv(ee_T_tt))) #Get the constrained orientation of the ee temp_q = quaternions.qmult([q_table[3], q_table[0], q_table[1], q_table[2]], [ 0.5, -0.5, -0.5, 0.5]) rot_ctrl = np.array([temp_q[1], temp_q[2], temp_q[3], temp_q[0]]) if self.block_gripper: gripper_ctrl = 0 # #Make sure the new pos doesn't go out of bounds!!! upper_bound = self.initial_pos + self.target_range + 0.01 lower_bound = self.initial_pos - self.target_range - 0.01 pos_ee = np.clip(pos_ee, lower_bound, upper_bound) self.psm.setPoseAtEE(pos_ee, rot_ctrl, gripper_ctrl)
def diff_drive(self, robot, index, target_pose):
"""
this diff drive is very hacky
it tries to transport the target pose to match an end pose
by computing the pose difference between current pose and target pose
then it estimates a cartesian velocity for the end effector to follow.
It uses differential IK to compute the required joint velocity, and set
the joint velocity as current step target velocity.
This technique makes the trajectory very unstable but it still works some times.
"""
pf = robot.get_compute_functions()['passive_force'](True, True, False)
max_v = 0.1
max_w = np.pi
qpos, qvel, poses = robot.get_observation()
current_pose: Pose = poses[index]
delta_p = target_pose.p - current_pose.p
delta_q = qmult(target_pose.q, qinverse(current_pose.q))
axis, theta = quat2axangle(delta_q)
if (theta > np.pi):
theta -= np.pi * 2
t1 = np.linalg.norm(delta_p) / max_v
t2 = theta / max_w
t = max(np.abs(t1), np.abs(t2), 0.001)
thres = 0.1
if t < thres:
k = (np.exp(thres) - 1) / thres
t = np.log(k * t + 1)
v = delta_p / t
w = theta / t * axis
target_qvel = robot.get_compute_functions()['cartesian_diff_ik'](
np.concatenate((v, w)), 9)
robot.set_action(qpos, target_qvel, pf)
def diff_drive2(self, robot, index, target_pose, js1, joint_target, js2):
"""
This is a hackier version of the diff_drive
It uses specified joints to achieve the target pose of the end effector
while asking some other specified joints to match a global pose
"""
pf = robot.get_compute_functions()['passive_force'](True, True, False)
max_v = 0.1
max_w = np.pi
qpos, qvel, poses = robot.get_observation()
current_pose: Pose = poses[index]
delta_p = target_pose.p - current_pose.p
delta_q = qmult(target_pose.q, qinverse(current_pose.q))
axis, theta = quat2axangle(delta_q)
if (theta > np.pi):
theta -= np.pi * 2
t1 = np.linalg.norm(delta_p) / max_v
t2 = theta / max_w
t = max(np.abs(t1), np.abs(t2), 0.001)
thres = 0.1
if t < thres:
k = (np.exp(thres) - 1) / thres
t = np.log(k * t + 1)
v = delta_p / t
w = theta / t * axis
target_qvel = robot.get_compute_functions()['cartesian_diff_ik'](
np.concatenate((v, w)), 9)
for j, target in zip(js2, joint_target):
qpos[j] = target
robot.set_action(qpos, target_qvel, pf)
def optimize_poses(pred_poses, vos=None, fc_vos=False, target_poses=None,
sax=1, saq=1, srx=1, srq=1):
"""
optimizes poses using either the VOs or the target poses (calculates VOs
from them)
:param pred_poses: N x 7
:param vos: (N-1) x 7
:param fc_vos: whether to use relative transforms between all frames in a fully
connected manner, not just consecutive frames
:param target_poses: N x 7
:param: sax: covariance of pose translation (1 number)
:param: saq: covariance of pose rotation (1 number)
:param: srx: covariance of VO translation (1 number)
:param: srq: covariance of VO rotation (1 number)
:return:
"""
pgo = PoseGraphFC() if fc_vos else PoseGraph()
if vos is None:
if target_poses is not None:
# calculate the VOs (in the pred_poses frame)
vos = np.zeros((len(target_poses) - 1, 7))
for i in range(len(vos)):
vos[i, :3] = target_poses[i + 1, :3] - target_poses[i, :3]
q0 = target_poses[i, 3:]
q1 = target_poses[i + 1, 3:]
vos[i, 3:] = txq.qmult(txq.qinverse(q0), q1)
else:
print('Specify either VO or target poses')
return None
optim_poses = pgo.optimize(poses=pred_poses, vos=vos, sax=sax, saq=saq,
srx=srx, srq=srq)
return optim_poses
def process_poses_quaternion(xyz_in, q_in, mean_t, std_t, align_R, align_t, align_s):
"""
processes the 1x12 raw pose from dataset by aligning and then normalizing
:param poses_in: N x 12
:param mean_t: 3
:param std_t: 3
:param align_R: 3 x 3
:param align_t: 3
:param align_s: 1
:return: processed poses (translation + quaternion) N x 7
"""
poses_out = np.zeros((len(xyz_in), 6))
poses_out[:, 0:3] = xyz_in
align_q = txq.mat2quat(align_R)
# align
for i in range(len(poses_out)):
q = txq.qmult(align_q, q_in[i])
q *= np.sign(q[0]) # constrain to hemisphere
q = qlog(q)
poses_out[i, 3:] = q
t = poses_out[i, :3] - align_t
poses_out[i, :3] = align_s * \
np.dot(align_R, t[:, np.newaxis]).squeeze()
# normalize translation
poses_out[:, :3] -= mean_t
poses_out[:, :3] /= std_t
return poses_out
def random_tilt(rob_arm, obj_name_list, min_tilt_degree, max_tilt_degree):
### method 1
#rot_dir = np.random.normal(size=(3,))
#rot_dir = rot_dir / np.linalg.norm(rot_dir)
### method 2
u = random.uniform(0, 1)
v = random.uniform(0, 1)
theta = 2 * math.pi * u
phi = math.acos(2 * v - 1)
x = math.sin(theta) * math.sin(phi)
y = math.cos(theta) * math.sin(phi)
z = math.cos(phi)
rot_dir = np.array([x, y, z])
rot_dir = rot_dir / np.linalg.norm(rot_dir)
tilt_degree = random.uniform(min_tilt_degree, max_tilt_degree)
print('rot_dir:', rot_dir)
print('tilt degree:', tilt_degree)
w = math.cos(math.radians(tilt_degree / 2))
x = math.sin(math.radians(tilt_degree / 2)) * rot_dir[0]
y = math.sin(math.radians(tilt_degree / 2)) * rot_dir[1]
z = math.sin(math.radians(tilt_degree / 2)) * rot_dir[2]
rot_quat = [w, x, y, z]
for obj_name in obj_name_list:
obj_quat = rob_arm.get_object_quat(obj_name) # [x,y,z,w]
obj_quat = [obj_quat[3], obj_quat[0], obj_quat[1],
obj_quat[2]] # change to [w,x,y,z]
obj_quat = qmult(rot_quat, obj_quat) # [w,x,y,z]
obj_quat = [obj_quat[1], obj_quat[2], obj_quat[3],
obj_quat[0]] # change to [x,y,z,w]
rob_arm.set_object_quat(obj_name, obj_quat)
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 obtain_v_dqs(ndat, delta, q):
out=np.zeros((ndat,3))
for i in range(ndat):
j=i+delta
#Since everything is squared does quat_reduce matter? ...no, but do so anyway.
dq=qs.quat_reduce(qops.qmult( qops.qinverse(q[i]), q[j]))
out[i]=dq[1:4]
return out
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_qmult_vectorisation(random_quats):
q1 = np.asarray([2, 3, 4, 5])
fast = vectorised_qmult(q1, random_quats)
stored_quat = np.ones_like(random_quats)
for i, row in enumerate(random_quats):
stored_quat[i] = qmult(q1, row)
assert np.allclose(fast, stored_quat)
def rotate_quat_by_euler(xyzw, e_x, e_y, e_z):
"""
wxyz: transforms3s array format
xyzw: pybullet format
"""
wxyz = PhysicsObject.quatXyzwToWxyz(xyzw)
rot_mat = euler.euler2mat(e_x, e_y, e_z)
wxyz = quaternions.qmult(rot_mat, wxyz)
return PhysicsObject.quatWxyzToXyzw(wxyz)
def inv_transform(pose_b, frame_b_a):
"""Inverse transforms a quaternion pose between reference frames.
Transforms a pose in reference frame B to a pose in reference frame
A (B is expressed relative to A).
"""
pos_a = rotate_vector(pose_b[:3], frame_b_a[3:]) + frame_b_a[:3]
rot_a = qmult(frame_b_a[3:], pose_b[3:])
pose_a = np.concatenate((pos_a, rot_a))
return pose_a
def transform(pose_a, frame_b_a):
"""Transforms a quaternion pose between reference frames.
Transforms a pose in reference frame A to a pose in reference frame
B (B is expressed relative to reference frame A).
"""
pos_b = rotate_vector(pose_a[:3] - frame_b_a[:3], qinverse(frame_b_a[3:]))
rot_b = qmult(qinverse(frame_b_a[3:]), pose_a[3:])
pose_b = np.concatenate((pos_b, rot_b))
return pose_b
def transformRovioQuaternionToViconCF(rovio_quat):
"""
Rotates a rovio imu quaternion (in imu world frame)
into the vicon world coordinate frame
"""
rot = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
rel_quat = mat2quat(rot)
new_quat = qmult(rovio_quat, rel_quat)
#print "Eulers of imu in vicon world:", [math.degrees(i) for i in quat2euler(new_quat)]
return new_quat
def _step(self, a):
self.nframe += 1
if not self.scene.multiplayer: # if multiplayer, action first applied to all robots, then global step() called, then _step() for all robots with the same actions
self.robot.apply_action(a)
self.scene.global_step()
state = self.robot.calc_state() # also calculates self.joints_at_limit
#self.rewards, done = self.calc_rewards_and_done(a, state)
self.rewards = self._rewards(a)
done = self._termination(state)
debugmode = 0
if (debugmode):
print("rewards=")
print(self.rewards)
print("sum rewards")
print(sum(self.rewards))
self.HUD(state, a, done)
self.reward += sum(self.rewards)
self.eps_reward += sum(self.rewards)
debugmode = 0
if debugmode:
print("Eps frame {} reward {}".format(self.nframe, self.reward))
print("position", self.robot.get_position())
if self.gui:
pos = self.robot.get_scaled_position()
orn = self.robot.get_orientation()
pos = (pos[0], pos[1], pos[2] + self.tracking_camera['z_offset'])
p.resetDebugVisualizerCamera(self.tracking_camera['distance'],
self.tracking_camera['yaw'],
self.tracking_camera['pitch'], pos)
eye_pos = self.robot.eyes.current_position()
debugmode = 0
if debugmode:
print("Camera env eye position", eye_pos)
x, y, z, w = self.robot.eyes.current_orientation()
eye_quat = quaternions.qmult([w, x, y, z], self.robot.eye_offset_orn)
debugmode = 0
if (debugmode):
print("episode rewards", sum(self.rewards), "steps", self.nframe)
#print(self.reward, self.rewards, self.robot.walk_target_dist_xyz)
episode = None
if done:
episode = {'r': self.reward, 'l': self.nframe}
debugmode = 0
if debugmode:
print("return episode:", episode)
return state, sum(self.rewards), bool(done), dict(eye_pos=eye_pos,
eye_quat=eye_quat,
episode=episode)
def get_allocentric_poses(self):
poses = self.get_poses()
poses_allocentric = []
for pose in poses:
dx = np.arctan2(pose[0], -pose[2])
dy = np.arctan2(pose[1], -pose[2])
quat = euler2quat(-dy, -dx, 0, axes='sxyz')
quat = qmult(qinverse(quat), pose[3:])
poses_allocentric.append(np.concatenate([pose[:3], quat]))
#print(quat, pose[3:], pose[:3])
return poses_allocentric
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 test_qnorm():
qi = tq.qeye()
assert tq.qnorm(qi) == 1
assert tq.qisunit(qi)
qi[1] = 0.2
assert not tq.qisunit(qi)
# Test norm is sqrt of scalar for multiplication with conjugate.
# https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal
for q in quats:
q_c = tq.qconjugate(q)
exp_norm = np.sqrt(tq.qmult(q, q_c)[0])
assert np.allclose(tq.qnorm(q), exp_norm)
def test_qnorm():
qi = tq.qeye()
assert tq.qnorm(qi) == 1
assert tq.qisunit(qi)
qi[1] = 0.2
assert not tq.qisunit(qi)
# Test norm is sqrt of scalar for multiplication with conjugate.
# https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal
for q in quats:
q_c = tq.qconjugate(q)
exp_norm = np.sqrt(tq.qmult(q, q_c)[0])
assert np.allclose(tq.qnorm(q), exp_norm)
def test_qmult():
# Test that quaternion * same as matrix *
for M1, q1 in eg_pairs[0::4]:
for M2, q2 in eg_pairs[1::4]:
q21 = tq.qmult(q2, q1)
assert_array_almost_equal(np.dot(M2,M1), tq.quat2mat(q21))
