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)