def reset(self):
self.total_box_genreated += 10
self.local_total_timesteps = 0
if self.left_robot:
self.scene.remove_articulation(self.left_robot.robot)
self.scene.remove_articulation(self.right_robot.robot)
base_size = np.array([0.015, 0.07, 0.07])
size = base_size + np.random.random(3) * np.array([0.01, 0.06, 0.06])
self.left_robot = Robot(
self.load_robot(Pose([-.5, .25, 0.6], axangle2quat([0, 0, 1], -np.pi / 2)), size))
self.left_robot.robot.set_qpos([1.5, 1, 0, -1, 0, 0, 0])
self.right_robot = Robot(
self.load_robot(Pose([-.5, -.25, 0.6], axangle2quat([0, 0, 1], np.pi / 2)), size))
self.right_robot.robot.set_qpos([-1.5, 1, 0, -1, 0, 0, 0])
self.create_bin([-1.2 + np.random.random() * 0.1,
np.random.random() * 0.2 - 0.1, 0.6],
np.random.random() * np.pi, [
0.2 + np.random.random() * 0.05, 0.3 + np.random.random() * 0.05,
0.4 + np.random.random() * 0.05
], 0.015 + np.random.random() * 0.05)
for d in self.boxes:
d.set_pose(Pose([np.random.random() * 0.2 - 0.1, np.random.random() * 0.4 - 0.2, 2]))
self.init()
self.scene.update_render()
def test_angle_axis2quat():
q = tq.axangle2quat([1, 0, 0], 0)
assert_array_equal(q, [1, 0, 0, 0])
q = tq.axangle2quat([1, 0, 0], np.pi)
assert_array_almost_equal(q, [0, 1, 0, 0])
q = tq.axangle2quat([1, 0, 0], np.pi, True)
assert_array_almost_equal(q, [0, 1, 0, 0])
q = tq.axangle2quat([2, 0, 0], np.pi, False)
assert_array_almost_equal(q, [0, 1, 0, 0])
def test_angle_axis2quat():
q = tq.axangle2quat([1, 0, 0], 0)
assert_array_equal(q, [1, 0, 0, 0])
q = tq.axangle2quat([1, 0, 0], np.pi)
assert_array_almost_equal(q, [0, 1, 0, 0])
q = tq.axangle2quat([1, 0, 0], np.pi, True)
assert_array_almost_equal(q, [0, 1, 0, 0])
q = tq.axangle2quat([2, 0, 0], np.pi, False)
assert_array_almost_equal(q, [0, 1, 0, 0])
def rotation(value, r):
q0 = quaternions.axangle2quat(
[float(value[0]), float(value[1]),
float(value[2])], float(value[3]))
q1 = quaternions.axangle2quat([r[0], r[1], r[2]], r[3])
qr = quaternions.qmult(q1, q0)
v, theta = quaternions.quat2axangle(qr)
return [
WebotsParser.str(v[0]),
WebotsParser.str(v[1]),
WebotsParser.str(v[2]),
WebotsParser.str(theta)
]
def test_cfg(cls):
np.random.seed(0)
xyz = np.zeros((7, ))
xyz[:3] = np.random.random((3, )) - 0.5
xyz[3:] = axangle2quat(np.random.random((3, )),
np.random.random() * np.pi * 2)
primitive = cls(cfg=cls.default_config())
grid_pos = ti.Vector.field(3, shape=(), dtype=ti.f64)
ans = ti.Vector.field(3, shape=(), dtype=ti.f64)
primitive.set_state(0, xyz)
print('finish int')
@ti.kernel
def gt():
ans[None] = primitive.normal2(0, grid_pos[None])
@ti.kernel
def sdf():
ans[None] = primitive.normal(0, grid_pos[None])
def run(func, a):
grid_pos[None] = a
func()
return ans.to_numpy()
passed = 0
while True:
passed += 1
if passed % 1000 == 0:
print('passed', passed)
a = np.random.random((3, )) * 2 - 1
assert abs(run(gt, a) - run(sdf, a)).max() < 1e-5, f"{cls}, {a}"
def to_euler_xyz(x):
x[0], x[1], x[2] = x[2], x[0], x[1]
th = np.linalg.norm(x)
x_norm = x / th
q = quaternions.axangle2quat(x, th)
a = euler.quat2euler(q)
return a
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 random_rotate_coordinates(coords):
"""
Returns a random rotation of the coordinates matrix.
Given a list of `n_particles` coordinates a random rotation axis is defined
and all coordinates are rotated about the axis.
Args:
coords (np.ndarray): `(n_particles,3)`array of coordinates
Returns:
np.ndarray: shape `(n_particles,3)` original coordinates rotated
"""
theta = 2.0 * np.pi * np.random.rand()
phi = np.arccos(1 - 2.0 * np.random.rand())
random_axis = np.array([
np.sin(phi) * np.cos(theta),
np.sin(phi) * np.sin(theta),
np.cos(phi)
])
random_quaternion = quaternions.axangle2quat(
random_axis, 2.0 * np.pi * np.random.randn())
rotated_coords = np.apply_along_axis(quaternions.rotate_vector,
1,
coords,
q=random_quaternion)
return rotated_coords
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 __init__(self,
viewer: Viewer,
hand_name: str = 'hand',
hand_model: HandModel = HandModel(),
thickness: float = 0.005):
"""Append the hand model to the viewer.
Arguments:
viewer {Viewer} -- viewer to attach the visualization
Keyword Arguments:
hand_name {str} -- hand name on the scene (default: {'hand'})
hand_model {HandModel} -- hand model (default: {HandModel()})
thickness {float} -- bones thickness (default: {0.005})
"""
self._viewer = viewer
self._name = hand_name
self._model = hand_model
self._clock = 0.0
self._viewer.append_group(self._name)
for link in self._model.links:
frame = ((-link.length / 2.0, 0.0, 0.0),
axangle2quat((0.0, -1.0, 0.0), np.pi / 2.0))
self._viewer.append_capsule(self._name, link.name, thickness,
link.length, frame)
def create_bin(self, p, r, size, thickness):
if self.bin is not None:
self.scene.remove_actor(self.bin)
self.bin_size = size
b = self.scene.create_actor_builder()
b.add_box_shape(Pose([0, 0, thickness / 2]), [size[0] / 2, size[1] / 2, thickness / 2])
b.add_box_visual(Pose([0, 0, thickness / 2]), [size[0] / 2, size[1] / 2, thickness / 2],
[0.1, 0.8, 1])
b.add_box_shape(Pose([0, size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2])
b.add_box_visual(Pose([0, size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2], [0.1, 0.8, 1])
b.add_box_shape(Pose([0, -size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2])
b.add_box_visual(Pose([0, -size[1] / 2, size[2] / 2]),
[size[0] / 2 + thickness / 2, thickness / 2, size[2] / 2], [0.1, 0.8, 1])
b.add_box_shape(Pose([size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2])
b.add_box_visual(Pose([size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2],
[0.1, 0.8, 1])
b.add_box_shape(Pose([-size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2])
b.add_box_visual(Pose([-size[0] / 2, 0, size[2] / 2]), [thickness / 2, size[1] / 2, size[2] / 2],
[0.1, 0.8, 1])
self.bin = b.build(True, name="bin")
self.bin.set_pose(Pose(p, axangle2quat([0, 0, 1], r)))
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 get_orientation(self, waypoint_id):
return Orientation.new_data(quaternion=dict(
zip(["w", "x", "y", "z"],
axangle2quat([0, 0, 1], self.get_yaw(waypoint_id)))),
rpy=Rpy.new_data(
roll=None,
pitch=None,
yaw=self.get_yaw(waypoint_id)))
def callback(msg):
grasps = msg.grasps
if len(grasps) > 0:
print("\n")
# rospy.loginfo('Received %d grasps.', len(grasps))
print "\033[0;32m%s\033[0m" % ("[INFO GRASP] Received " +
str(len(grasps)) + " grasps")
# print grasps
# print grasps[0].score.data
print "\033[0;32m%s\033[0m" % ("[INFO GRASP] Score: " +
str(grasps[0].score.data))
# rot = np.array([ (grasps[0].approach.x, grasps[0].approach.y, grasps[0].approach.z),
# (grasps[0].binormal.x, grasps[0].binormal.y, grasps[0].binormal.z),
# (grasps[0].axis.x, grasps[0].axis.y, grasps[0].axis.z) ])
if grasps[0].score.data > min_score:
# 旋转矩阵的第一列为坐标系x轴方向向量，第二列为坐标系y轴方向向量，第三列为坐标系z轴方向向量
rot = np.array([
(grasps[0].approach.x, grasps[0].binormal.x, grasps[0].axis.x),
(grasps[0].approach.y, grasps[0].binormal.y, grasps[0].axis.y),
(grasps[0].approach.z, grasps[0].binormal.z, grasps[0].axis.z)
])
print "\033[0;32m%s\033[0m" % "[INFO GRASP] Rotation matrix:\n", rot
quaternion = quaternions.mat2quat(rot) # [w, x, y, z]
quaternion_y = quaternions.axangle2quat([0, 1, 0],
np.pi / 2) # 绕y轴旋转90度的四元数
quaternion_z = quaternions.axangle2quat([0, 0, 1],
np.pi / 2) # 绕z轴旋转90度的四元数
# 四元数相乘，即将原抓取姿态绕y轴旋转90度, 再绕z轴旋转90度，目的为使抓取姿态坐标系与末端执行器坐标系匹配
quaternion = quaternions.qmult(quaternion, quaternion_y)
# quaternion = quaternions.qmult(quaternion, quaternion_z)
print "\033[0;32m%s\033[0m" % "[INFO GRASP] Quaternion:", quaternion, "\n"
br = tf.TransformBroadcaster()
br.sendTransform(
(grasps[0].position.x, grasps[0].position.y,
grasps[0].position.z), # (x, y , z)
(quaternion[1], quaternion[2], quaternion[3],
quaternion[0]), # (x, y, z, w)
rospy.Time.now(),
"grasp",
camera_frame)
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 test_mat2rot6d_np():
axis = np.random.rand(3)
angle = np.random.rand(1)
# quat = axangle2quat([1, 2, 3], 0.7)
quat = axangle2quat(axis, angle)
print("quat:\n", quat)
mat = quat2mat(quat)
print("mat:\n", mat)
rot6d = mat_to_ortho6d_np(mat)
print("rot6d: \n", rot6d)
def _interpolate_transformation(t_1, t_2, coefficient):
assert 0 <= coefficient < 1
t_result = np.identity(4)
t_result[0:3, 3] = t_1[0:3, 3] * (1 - coefficient) + t_2[0:3, 3] * coefficient
rot_diff = t_2[0:3, 0:3].dot(t_1[0:3, 0:3].transpose())
q_diff = quaternions.mat2quat(rot_diff)
axis, angle = quaternions.quat2axangle(q_diff)
rot_delta = quaternions.quat2mat(quaternions.axangle2quat(axis, angle * coefficient))
rot_result = rot_delta.dot(t_1[0:3, 0:3])
t_result[0:3, 0:3] = rot_result
return t_result
def test_axis_angle():
for M, q in eg_pairs:
vec, theta = tq.quat2axangle(q)
q2 = tq.axangle2quat(vec, theta)
assert tq.nearly_equivalent(q, q2)
aa_mat = taa.axangle2mat(vec, theta)
assert_array_almost_equal(aa_mat, M)
aa_mat2 = sympy_aa2mat(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat2)
aa_mat22 = sympy_aa2mat2(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat22)
def test_axis_angle():
for M, q in eg_pairs:
vec, theta = tq.quat2axangle(q)
q2 = tq.axangle2quat(vec, theta)
assert tq.nearly_equivalent(q, q2)
aa_mat = taa.axangle2mat(vec, theta)
assert_array_almost_equal(aa_mat, M)
aa_mat2 = sympy_aa2mat(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat2)
aa_mat22 = sympy_aa2mat2(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat22)
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()
in_x = [-0.1, 0.1]
in_y = [-0.1, 0.1]
in_r = [-0.1, 0.1]
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"]
if self.config["curriculum"]:
if not (self.env.eps_count - 1) % self.config['n_batch']:
if self.config["enjoy"]:
self.iter_so_far = 150
scale_fac = 5
self.iter_so_far += 1
self.config["random"]["random_init_x_range"] = [
k * float(self.iter_so_far / scale_fac) for k in in_x
]
self.config["random"]["random_init_y_range"] = [
k * float(self.iter_so_far / scale_fac) for k in in_y
]
self.config["random"]["random_init_rot_range"] = [
k * float(self.iter_so_far / scale_fac) for k in in_r
]
#print("X_range:{}, Y_range:{}".format(x_range,y_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])
]
#print("X_range:{}, Y_range:{}, new_Pose:{}".format(x_range, y_range, new_pos))
#print("Iteration:", self.iter_so_far)
#print("r_range:{}".format(r_range))
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 _set_action(self, action):
# self.sim.data.qvel[:] = 0
# self.sim.data.qacc[:] = 0
# To do grav comp
# self.sim.data.qfrc_applied[:] = self.sim.data.qfrc_bias
obj_pos = self.sim.data.get_body_xpos('robot0:grip')
obj_quat = self.sim.data.get_body_xquat('robot0:grip')
# import IPython; IPython.embed(); import sys; sys.exit(0)
# self.angle = quaternions.quat2axangle(obj_quat)
action = np.clip(action, self.action_space.low, self.action_space.high)
action, lift = action[:3], action[3]
action = action.copy()
limit_satisfied = self.limit_satisfied()
rot_ctrl = quaternions.axangle2quat([1, 0, .0], self.angle)
if not self.object_in_contact:
# Once lifted you can only lift or hold
if lift <= 0:
pos_ctrl = np.zeros(3)
action = np.concatenate([pos_ctrl, rot_ctrl])
else:
pos_ctrl = np.array([0, 0, 0.01])
action = np.concatenate([pos_ctrl, rot_ctrl])
else:
pos_ctrl, rot_ctrl = action[:2], action[2]
pos_ctrl *= 0.05 # limit maximum change in position
rot_ctrl *= 0.05 # limit maximum change in rotation
if lift <= 0:
pos_ctrl = np.hstack([pos_ctrl, [-0.001]])
else:
pos_ctrl = np.hstack([pos_ctrl, [0.01]])
self.angle += rot_ctrl
rot_ctrl = quaternions.axangle2quat([1, 0, .0], self.angle)
action = np.concatenate([pos_ctrl, rot_ctrl])
# print("action", action)
utils.mocap_set_action(self.sim, action)
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 quat_v1v2(v1, v2):
"""
Return the minimum-angle quaternion that rotates v1 to v2.
Non-SIMD version for clarity of maths.
"""
th = acos(np.dot(v1, v2))
ax = np.cross(v1, v2)
if all(np.isnan(ax)):
# = = = This happens when the two vectors are identical
return qops.qeye()
else:
# = = = Do normalisation within the next function
return qops.axangle2quat(ax, th)
def random_quaternion():
"""Random uniformly distributed quaternion
A quaternion defines an 3d axis of rotation and a corresponding angle
of rotation. A random quaternion is drawn by first drawing a random
rotation axis from the uniform random sphere and a uniform angle. Random
quaternions are used for random rotation operations on arbitrary geometries.
Returns:
transforms3d.Quaternion: a random quaternion from a uniform distribution
"""
return quaternions.axangle2quat(random_point_on_unit_sphere(),
2.0 * np.pi * np.random.randn())
def new_orientation_from_dir(orn, next_dir):
initial_dir = np.array([1, 0])
cos = np.dot(initial_dir, normalize(next_dir))
sin = np.cross(initial_dir, normalize(next_dir))
rad = np.arccos(cos)
if sin < 0:
rad = -rad
# print(rad)
next_orn = quatToXYZW(axangle2quat(np.array([0, 0, 1]), rad, is_normalized=True), 'wxyz')
return next_orn
def rot_around_axis(quat, angle, axis):
if axis == 0:
axis_vec = [1, 0, 0]
elif axis == 1:
axis_vec = [0, 1, 0]
elif axis == 2:
axis_vec = [0, 0, 1]
quat_in = (quat[3], quat[0], quat[1], quat[2]) # [w,x,y,z]
quat_trans = quaternions.axangle2quat(axis_vec, angle)
quat_out = quaternions.qmult(quat_in, quat_trans)
quat_out = (quat_out[1], quat_out[2], quat_out[3], quat_out[0])
return quat_out
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 reset_random_pos(self):
'''Add randomness to resetted initial position
'''
if not self.env.config["random"]["random_initial_pose"]:
return
pos = self.robot_body.current_position()
orn = self.robot_body.current_orientation()
new_pos = [ pos[0] + self.np_random.uniform(low=-delta_pos, high=delta_pos),
pos[1] + self.np_random.uniform(low=-delta_pos, high=delta_pos),
pos[2] + self.np_random.uniform(low=0, high=delta_pos)]
new_orn = quaternions.qmult(quaternions.axangle2quat([1, 0, 0], self.np_random.uniform(low=-delta_orn, high=delta_orn)), orn)
self.robot_body.reset_orientation(new_orn)
self.robot_body.reset_position(new_pos)
def test_ortho6d_rot():
axis = np.random.rand(3)
angle = np.random.rand(1)
# quat = axangle2quat([1, 2, 3], 0.7)
quat = axangle2quat(axis, angle)
print("quat:\n", quat)
mat = quat2mat(quat)
print("mat:\n", mat)
mat_th = torch.tensor(mat.astype("float32"))[None].to("cuda")
print("mat_th:\n", mat_th)
ortho6d = mat_to_ortho6d_batch(mat_th)
print("ortho6d:\n", ortho6d)
mat_2 = ortho6d_to_mat_batch(ortho6d)
print("mat_2:\n", mat_2)
diff_mat = mat_th - mat_2
print("mat_diff:\n", diff_mat)
def test_mat2quat_torch():
from core.utils.pose_utils import quat2mat_torch
axis = np.random.rand(3)
angle = np.random.rand(1)
# quat = axangle2quat([1, 2, 3], 0.7)
quat = axangle2quat(axis, angle)
print("quat:\n", quat)
mat = quat2mat(quat)
print("mat:\n", mat)
mat_th = torch.tensor(mat.astype("float32"))[None].to("cuda")
print("mat_th:\n", mat_th)
quat_th = mat2quat_batch(mat_th)
print("quat_th:\n", quat_th)
mat_2 = quat2mat_torch(quat_th)
print("mat_2:\n", mat_2)
diff_mat = mat_th - mat_2
print("mat_diff:\n", diff_mat)
diff_quat = quat - quat_th.cpu().numpy()
print("diff_quat:\n", diff_quat)
def _env_setup(self, initial_qpos):
print("-------------------- ENV SETUP -------------------------")
self.angle = -np.pi / 4.0 + np.clip(np.random.normal(), -0.3, 0.3)
# change the weight of the object
body_id = self.sim.model.body_name2id('object0')
mass = np.random.choice(8) # mass is from 4 to 12
self.sim.model.body_mass[body_id] = mass + 4.0
self.body_mass_obs = np.zeros(8, dtype=np.float32)
self.body_mass_obs[mass] = 1.0
self.mass = mass + 4.0
for name, value in initial_qpos.items():
self.sim.data.set_joint_qpos(name, value)
self.sim.data.qpos[:] = np.array(
[5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43])
utils.reset_mocap_welds(self.sim)
self.sim.step()
# # Move end effector into position.
target_xy = (np.random.uniform(size=2) - 0.5) * 0.5
# gripper_target = np.array([target_xy[0], target_xy[1] - 0.4, -0.02]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_target = np.array([
target_xy[0], target_xy[1], -0.02
]) + self.sim.data.get_site_xpos('robot0:grip')
gripper_rotation = np.array([1., 0., 1., 0.])
gripper_rotation /= np.linalg.norm(gripper_rotation)
quat_delta = quaternions.axangle2quat([1, 0, .0], self.angle)
gripper_rotation = quaternions.qmult(gripper_rotation, quat_delta)
for _ in range(100):
self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)
self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)
self.sim.step()
utils.reset_mocap2body_xpos(self.sim)
self.initial_qpos = self.sim.data.qpos.copy()
def animate(shape,
rotation,
mutators=[],
delay=.15,
n=400,
size=(170, 70),
smooth=4.):
"""
Animates the shape passed in into the console
Parameters:
shape (Shape): The shape to render
rotation (Rotation): How the object should rotate within the view
delay (float): seconds between ticks
n (int > 0): number of points to render
size (tuple(int > 0, int > 0)): size of the view, in characters
smooth (float): frames per tick
"""
i = 1
q = rotation.initialQuaternion
rotateBy = quats.axangle2quat(rotation.moveAngle,
rotation.speed * tau / smooth)
texture = Texture(theta=tau / sqrt(2))
while True:
frameStart = time.time()
q = quats.qmult(q, rotateBy)
# print(q)
[mut(shape=shape, i=i / smooth, texture=texture) for mut in mutators]
points = makePoints(texture.theta, n, shape)
if (i != 1): sys.stdout.write("\033[F" * (size[1] + 1))
shape.adjustDensity(points)
ViewPoints(points, size, rotateQuaternion=q).render()
# print(theta)
i += 1
frameDuration = time.time() - frameStart
sleepTime = delay / smooth - max(0, frameDuration)
if sleepTime > 0: time.sleep(sleepTime)
