def set_target_object(self, random_object=False, random_position=False): if random_object: rand_object = random.choice(self.object_list) self.object_name = rand_object["name"] self.object_type_str = rand_object["type"] self.object_type = self.object_list.index(rand_object) self.object_height = rand_object["height"] init_pos = rand_object["init_pos"] self.object_initial_position = Pose(position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]), orientation=quaternion_from_euler(init_pos[3], init_pos[4], init_pos[5])) else: self.object_name = self.object_list[0]["name"] self.object_type_str = self.object_list[0]["type"] self.object_type = 0 self.object_height = self.object_list[0]["height"] init_pos = self.object_list[0]["init_pos"] self.object_initial_position = Pose(position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]), orientation=quaternion_from_euler(init_pos[3], init_pos[4], init_pos[5])) if random_position: if self.object_type_str == "door_handle": box_pos = U.get_random_door_handle_pos() else: box_pos = Pose(position=Point(x=np.random.uniform(low=-0.3, high=0.3, size=None), y=np.random.uniform(low=0.9, high=1.1, size=None), z=1.05), orientation=quaternion_from_euler(0, 0, 0)) else: box_pos = self.object_initial_position U.change_object_position(self.object_name, box_pos) print("Current target: ", self.object_name)
def test_quat(self): euler1 = np.random.rand(3) * 2.0 * np.pi - np.pi euler2 = np.random.rand(3) * 2.0 * np.pi - np.pi q1 = tf.quaternion_from_euler(*euler1) q2 = tf.quaternion_from_euler(*euler2) q1q2_0 = tf.quaternion_multiply(q1, q2) q1q2_1 = (quat(*q1) * quat(*q2)).data self.assertTrue(np.allclose(q1q2_0, q1q2_1))
def _publish_tf(self, update_rate): # TODO Griffin: Update transforms with Vector measurements. Currently assumes old cozmo specs """ Broadcast current transformations and update measured velocities for odometry twist. Published transforms: odom_frame -> footprint_frame footprint_frame -> base_frame base_frame -> head_frame head_frame -> camera_frame camera_frame -> camera_optical_frame """ now = rospy.Time.now() x = self._vector.pose.position.x * 0.001 y = self._vector.pose.position.y * 0.001 z = self._vector.pose.position.z * 0.001 # compute current linear and angular velocity from pose change # Note: Sign for linear velocity is taken from commanded velocities! # Note: The angular velocity can also be taken from gyroscopes! dist = np.sqrt((self._last_pose.position.x - self._vector.pose.position.x)**2 + (self._last_pose.position.y - self._vector.pose.position.y)**2 + (self._last_pose.position.z - self._vector.pose.position.z)**2) / 1000.0 self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel) self._ang_vel = -(self._last_pose.rotation.angle_z.radians - self._vector.pose.rotation.angle_z.radians)* update_rate # publish odom_frame -> footprint_frame q = quaternion_from_euler(.0, .0, self._vector.pose_angle_rad) self._tfb.send_transform( (x, y, 0.0), q, now, self._footprint_frame, self._odom_frame) # publish footprint_frame -> base_frame q = quaternion_from_euler(.0, -self._vector.pose_pitch_rad, .0) self._tfb.send_transform( (0.0, 0.0, 0.02), q, now, self._base_frame, self._footprint_frame) # publish base_frame -> head_frame q = quaternion_from_euler(.0, -self._vector.head_angle_rad, .0) self._tfb.send_transform( (0.02, 0.0, 0.05), q, now, self._head_frame, self._base_frame) # publish head_frame -> camera_frame self._tfb.send_transform( (0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0), now, self._camera_frame, self._head_frame) # publish camera_frame -> camera_optical_frame q = self._optical_frame_orientation self._tfb.send_transform( (0.0, 0.0, 0.0), q, now, self._camera_optical_frame, self._camera_frame) # store last pose self._last_pose = deepcopy(self._vector.pose)
def _publish_tf(self, update_rate): """ Broadcast current transformations and update measured velocities for odometry twist. Published transforms: odom_frame -> footprint_frame footprint_frame -> base_frame base_frame -> head_frame head_frame -> camera_frame camera_frame -> camera_optical_frame """ #now = rospy.Time.now() now = TimeStamp.now() x = self._cozmo.pose.position.x * 0.001 y = self._cozmo.pose.position.y * 0.001 z = self._cozmo.pose.position.z * 0.001 # compute current linear and angular velocity from pose change # Note: Sign for linear velocity is taken from commanded velocities! # Note: The angular velocity can also be taken from gyroscopes! delta_pose = self._last_pose - self._cozmo.pose dist = np.sqrt(delta_pose.position.x**2 + delta_pose.position.y**2 + delta_pose.position.z**2) / 1000.0 self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel) self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate # publish odom_frame -> footprint_frame q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians) self._tfb.send_transform((x, y, 0.0), q, now, self._footprint_frame, self._odom_frame) # publish footprint_frame -> base_frame q = quaternion_from_euler(.0, -self._cozmo.pose_pitch.radians, .0) self._tfb.send_transform((0.0, 0.0, 0.02), q, now, self._base_frame, self._footprint_frame) # publish base_frame -> head_frame q = quaternion_from_euler(.0, -self._cozmo.head_angle.radians, .0) self._tfb.send_transform((0.02, 0.0, 0.05), q, now, self._head_frame, self._base_frame) # publish head_frame -> camera_frame self._tfb.send_transform((0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0), now, self._camera_frame, self._head_frame) # publish camera_frame -> camera_optical_frame q = self._optical_frame_orientation self._tfb.send_transform((0.0, 0.0, 0.0), q, now, self._camera_optical_frame, self._camera_frame) # store last pose self._last_pose = deepcopy(self._cozmo.pose)
def __init__(self): #code for camera initialisation self.orientation = transformations.quaternion_from_euler(0.0,0.0,0.0) self.input_orientation = transformations.quaternion_from_euler(0.0,0.0,0.0) self.position = np.array(self.POSITION) self.velocity = np.zeros(3) self.c_acceleration = np.zeros(3) self.t_last_update = time.time() self.set_cam_properties(75, 1.0, 1.0, 200000.0) #self.FOVangle = CameraObj.FOVY self.view = np.eye(4,dtype=np.float32)
def apply_joint_rotation_offset(self, joint_name, euler, frame_range=None, blend_window_size=None): motion = self.get_motion() print("euler ", euler) delta_q = quaternion_from_euler(*np.radians(euler)) if motion.frames.ndim == 1: motion.frames = motion.frames.reshape((1, len(motion.frames))) j_offset = self.get_skeleton().animated_joints.index( joint_name) * 4 + 3 if frame_range is None: start_frame = 0 end_frame = motion.n_frames else: start_frame, end_frame = frame_range end_frame = min(motion.n_frames, end_frame + 1) for idx in range(start_frame, end_frame): old_q = motion.frames[idx, j_offset:j_offset + 4] motion.frames[idx, j_offset:j_offset + 4] = quaternion_multiply( delta_q, old_q) if frame_range is not None and blend_window_size is not None: joint_list = [joint_name] offset = self.skeleton.nodes[ joint_name].quaternion_frame_index * 4 + 3 joint_index_list = list(range(offset, offset + 4)) motion.frames = apply_blending(self.skeleton, motion.frames, joint_list, joint_index_list, start_frame, end_frame - 1, blend_window_size)
def pose_command(self, px, py, pz, roll, pitch, yaw, *jnt_cmds): """ Same as set_pose but customized of OpenAI's GYM for a single call set method :param px: :param py: :param pz: :param roll: :param pitch: :param yaw: :param jnt_cmds: :return: """ # Edited python3 code quat = quaternion_from_euler(roll, pitch, yaw, 'szyx') # Initial python2 code # quat = transformations.quaternion_from_euler(roll, pitch, yaw, 'szyx') self._cmd.enable_position_controller = True self._cmd.pose.position.x = px self._cmd.pose.position.y = py self._cmd.pose.position.z = pz self._cmd.pose.orientation.x = quat[0] self._cmd.pose.orientation.y = quat[1] self._cmd.pose.orientation.z = quat[2] self._cmd.pose.orientation.w = quat[3] self._cmd.joint_cmds = [jnt for jnt in jnt_cmds] self._apply_command()
def back_front_thread(): global published_transform rate = rospy.Rate(10) while True: time = rospy.Time.now() num = 0 sum_mat = None #print("Time diff = %s" %(str(time.to_sec() - front_transform["time"].to_sec()) if front_transform["time"] is not None else "??")) if front_transform["transform"] is not None and time.to_sec( ) - front_transform["time"].to_sec() < 1: sum_mat = front_transform["transform"] num = 1.0 # BRS Let's not use averages - seems to cause normailization errors? if back_transform["transform"] is not None and time.to_sec( ) - back_transform["time"].to_sec() < 1: sum_mat = back_transform[ "transform"] if sum_mat is None else sum_mat + back_transform[ "transform"] num = num + 1.0 if num > 0: transform = sum_mat / num published_transform = transform trans = tr.translation_from_matrix(transform) rot = tr.quaternion_from_matrix(transform) r, p, y = tr.euler_from_quaternion(rot) rot = tr.quaternion_from_euler(r, p, y) br.sendTransform(trans, rot, time, "odom", "map") # elif published_transform is not None: # transform = published_transform # trans = tr.translation_from_matrix(transform) # rot = tr.quaternion_from_matrix(transform) # br.sendTransform(trans, rot, time, "odom", "map") rate.sleep()
def fill_from_Orientation_Data(o): '''Fill messages with information from 'Orientation Data' MTData2 block.''' self.pub_imu = True #self.imu_msg = self.imu_pub.initProto() try: x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0'] except KeyError: pass try: x, y, z, w = quaternion_from_euler(radians(o['Roll']), radians(o['Pitch']), radians(o['Yaw'])) except KeyError: pass try: a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\ o['e'], o['f'], o['g'], o['h'], o['i'] m = identity_matrix() m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i)) x, y, z, w = quaternion_from_matrix(m) except KeyError: pass w, x, y, z = convert_quat((w, x, y, z), o['frame']) roll, pitch, yaw = euler_from_quaternion((w, x, y, z)) self.imu_msg.angleRoll = roll self.imu_msg.anglePitch = pitch self.imu_msg.angleYaw = yaw
def gs_head_pose_info_to_msg(head_pose_info): """ """ msg = gazesense_msgs.msg.HeadPoseInfo() msg.is_lost = head_pose_info.is_lost if not head_pose_info.is_lost: msg.track_session_uid = head_pose_info.track_session_uid msg.transform.translation.x = head_pose_info.transform.translation[0] msg.transform.translation.y = head_pose_info.transform.translation[1] msg.transform.translation.z = head_pose_info.transform.translation[2] # Python 2/3 problem for tf.transformations R = np.zeros((4, 4), dtype=np.float) R[3, 3] = 1 R[:3, :3] = head_pose_info.transform.rotation euler = list(transformations.euler_from_matrix(R)) euler[1] *= -1 q = transformations.quaternion_from_euler(*euler) ############################################################## # UNCOMMENT WHEN tf.transformations CAN BE CALLED # q = tf.transformations.quaternion_from_matrix(R) # q = transformations.quaternion_from_matrix(R) ############################################################## msg.transform.rotation.x = q[0] msg.transform.rotation.y = q[1] msg.transform.rotation.z = q[2] msg.transform.rotation.w = q[3] return msg
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None): rospy.logdebug("joint: %s"%jointname) U = get_pr2_urdf() joint = U.joints[jointname] joint.origin = joint.origin or urdf.Pose() if not joint.origin.position: joint.origin.position = [0,0,0] if not joint.origin.rotation: joint.origin.rotation = [0,0,0] parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation)) childFromRotated = np.eye(4) if valuedict and jointname in valuedict: if joint.joint_type == 'revolute' or joint.joint_type == 'continuous': childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis) elif joint.joint_type == 'prismatic': childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname]) parentFromRotated = np.dot(parentFromChild, childFromRotated) originFromParent = conversions.pose_to_hmat(ps.pose) originFromRotated = np.dot(originFromParent, parentFromRotated) newps = gm.PoseStamped() newps.header = ps.header newps.pose = conversions.hmat_to_pose(originFromRotated) return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None): markers = [] U = get_pr2_urdf() link = U.links[linkname] if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh): rospy.logdebug("%s is a mesh. drawing it.", linkname) marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD) marker.ns = ns marker.header = ps.header linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation)) origFromLink = conversions.pose_to_hmat(ps.pose) origFromGeom = np.dot(origFromLink, linkFromGeom) marker.pose = conversions.hmat_to_pose(origFromGeom) marker.scale = gm.Vector3(1,1,1) marker.color = stdm.ColorRGBA(1,1,0,.5) marker.id = 0 marker.lifetime = rospy.Duration() marker.mesh_resource = str(link.visual.geometry.filename) marker.mesh_use_embedded_materials = False markers.append(marker) else: rospy.logdebug("%s is not a mesh", linkname) if U.child_map.has_key(linkname): for (cjoint,clink) in U.child_map[linkname]: markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict)) return markers
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None): rospy.logdebug("joint: %s" % jointname) U = get_pr2_urdf() joint = U.joints[jointname] joint.origin = joint.origin or urdf.Pose() if not joint.origin.position: joint.origin.position = [0, 0, 0] if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0] parentFromChild = conversions.trans_rot_to_hmat( joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation)) childFromRotated = np.eye(4) if valuedict and jointname in valuedict: if joint.joint_type == 'revolute' or joint.joint_type == 'continuous': childFromRotated = transformations.rotation_matrix( valuedict[jointname], joint.axis) elif joint.joint_type == 'prismatic': childFromRotated = transformations.translation_matrix( np.array(joint.axis) * valuedict[jointname]) parentFromRotated = np.dot(parentFromChild, childFromRotated) originFromParent = conversions.pose_to_hmat(ps.pose) originFromRotated = np.dot(originFromParent, parentFromRotated) newps = gm.PoseStamped() newps.header = ps.header newps.pose = conversions.hmat_to_pose(originFromRotated) return make_kin_tree_from_link(newps, joint.child, ns=ns, valuedict=valuedict)
def sendSetpoint(): global run setPointsCount = 0 local_setpoint_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=1) rate = rospy.Rate(20) while run: q = quaternion_from_euler(0, 0, 0, axes="sxyz") msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.header.seq = setPointsCount msg.pose.position.x = 0 msg.pose.position.y = 0 msg.pose.position.z = 0 msg.pose.orientation.x = q[0] msg.pose.orientation.y = q[1] msg.pose.orientation.z = q[2] msg.pose.orientation.w = q[3] local_setpoint_pub.publish(msg) setPointsCount = setPointsCount + 1 rate.sleep()
def getMarkerTFFromMap(m): # This doesn't work mid = "Marker%s" % m marker = marker_dict[mid] w_mat = None if marker is not None: w = 0 # east p = (-marker["x"], -marker["y"], -0.25) #east if marker["wall"] == "north": p = (marker["y"], -marker["x"], -0.25) w = math.pi / 2 elif marker["wall"] == "south": p = (-marker["y"], marker["x"], -0.25) w = -math.pi / 2 elif marker["wall"] == "west": p = (marker["x"], marker["y"], -0.25) w = math.pi q = tr.quaternion_from_euler(0, 0, w) print("Map: (%s) Marker%s: %s %s" % (marker["wall"], m, p, q)) t = tr.translation_matrix(p) r = tr.quaternion_matrix(q) w_mat = numpy.dot(t, r) return (w_mat, None)
def _publish_odometry(self): """ Publish current pose as Odometry message. """ # only publish if we have a subscriber if self._odom_pub.get_num_connections() == 0: return now = rospy.Time.now() odom = Odometry() odom.header.frame_id = self._odom_frame odom.header.stamp = now odom.child_frame_id = self._footprint_frame odom.pose.pose.position.x = self._vector.pose.position.x * 0.001 odom.pose.pose.position.y = self._vector.pose.position.y * 0.001 odom.pose.pose.position.z = self._vector.pose.position.z * 0.001 q = quaternion_from_euler(.0, .0, self._vector.pose_angle_rad) odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3, 1e-1]).ravel() odom.twist.twist.linear.x = self._lin_vel odom.twist.twist.angular.z = self._ang_vel odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3, 1e-2]).ravel() self._odom_pub.publish(odom)
def construct(self, x, y, w, name): self.x = x self.y = y self.w = w self.name = name match = re.search("Marker([0-9]+)", name) num = match.group(1) self.xml = MARKER_TEMPLATE.replace("$ID", num).replace("$WHO", user) pose = Pose() pose.position.x = x pose.position.y = y pose.position.z = 0.25 q = tf.quaternion_from_euler(0, 0, w) pose.orientation.x = q[0] pose.orientation.y = q[1] pose.orientation.z = q[2] pose.orientation.w = q[3] self.pose = pose # Generate obstacles name (in threadsafe manner) # Create gazebo request and call self.spawn_request = SpawnModelRequest() self.spawn_request.model_name = "Marker%s" % num self.spawn_request.model_xml = self.xml self.spawn_request.initial_pose = self.pose
def extract_geometric_mapping_data(*args): data = list() if len(args) == 1: obj = args[0] b_box_size_x = obj.init_bounding_box.max.x - obj.init_bounding_box.min.x b_box_size_y = obj.init_bounding_box.max.y - obj.init_bounding_box.min.y b_box_size_z = obj.init_bounding_box.max.z - obj.init_bounding_box.min.z pos_x = obj.init_position.x pos_y = obj.init_position.y pos_z = obj.init_position.z q = tf.quaternion_from_euler(np.radians(obj.init_rotation.roll), np.radians(obj.init_rotation.pitch), np.radians(obj.init_rotation.yaw)) data = [ b_box_size_x, b_box_size_y, b_box_size_z, pos_x, pos_y, pos_z, q.w, q.x, q.y, q.z ] elif len(args) == 2: obj1 = args[0] obj2 = args[1] b_box1_size_x = obj1.init_bounding_box.max.x - obj1.init_bounding_box.min.x b_box1_size_y = obj1.init_bounding_box.max.y - obj1.init_bounding_box.min.y b_box1_size_z = obj1.init_bounding_box.max.z - obj1.init_bounding_box.min.z q1 = tf.quaternion_from_euler( np.radians(obj1.init_rotation.roll), np.radians(obj1.init_rotation.pitch), np.radians(obj1.init_rotation.yaw )) #euler_to_quaternion(obj1.init_rotation) b_box2_size_x = obj2.init_bounding_box.max.x - obj2.init_bounding_box.min.x b_box2_size_y = obj2.init_bounding_box.max.y - obj2.init_bounding_box.min.y b_box2_size_z = obj2.init_bounding_box.max.z - obj2.init_bounding_box.min.z q2 = tf.quaternion_from_euler( np.radians(obj2.init_rotation.roll), np.radians(obj2.init_rotation.pitch), np.radians(obj2.init_rotation.yaw )) #euler_to_quaternion(obj2.init_rotation) t_rel = obj1.init_position - obj2.init_position data = [b_box1_size_x, b_box1_size_y, b_box1_size_z, b_box2_size_x, b_box2_size_y, b_box2_size_z, \ t_rel.x, t_rel.y, t_rel.z, \ q1[0], q1[1], q1[2], q1[3], q2[0], q2[1], q2[2], q2[3]] return np.array(data)
def euler2quaternion(self, euler): roll, pitch, yaw = euler q = transformations.quaternion_from_euler(-pitch+pi, -yaw, roll-pi, 'ryxz') # q = [0.0,0.0,0.0,0.0] # different between ros-tf(x,y,z,w) and transformations(w,x,y,z) quaternion = [q[1],q[2],q[3],q[0]] # different between ros-tf(x,y,z,w) and transformations(w,x,y,z) return (quaternion)
def ladder_helper(self, q0, a0, a1): q1 = transformations.quaternion_from_euler(-a1 * d2r, -a0 * d2r, 0.0, 'rzyx') q = transformations.quaternion_multiply(q1, q0) v = transformations.quaternion_transform(q, [1.0, 0.0, 0.0]) uv = self.project_point( [self.ned[0] + v[0], self.ned[1] + v[1], self.ned[2] + v[2]]) return uv
def hmat_to_trans_rot(hmat): ''' Converts a 4x4 homogenous rigid transformation matrix to a translation and a quaternion rotation. ''' _scale, _shear, angles, trans, _persp = transformations.decompose_matrix(hmat) rot = transformations.quaternion_from_euler(*angles) return trans, rot
def _convert_transform(origin): if origin is None: return tf.Transform3d() else: return tf.Transform3d(rot=torch.tensor(tf2.quaternion_from_euler( *origin.rpy, "sxyz"), dtype=torch.float32), pos=origin.xyz)
def compare(quat2rpy): eps = 1e-8 quat = tfs.quaternion_from_euler(0.19, 0.28, 0.38) rpy0 = quat2rpy(quat) print("rpy: {0}".format(rpy0)) quat[0]+=eps rpy1 = quat2rpy(quat) print("rpy: {0}".format(rpy1))
def hmat_to_trans_rot(hmat): ''' Converts a 4x4 homogenous rigid transformation matrix to a translation and a quaternion rotation. ''' scale, shear, angles, trans, persp = transformations.decompose_matrix(hmat) rot = transformations.quaternion_from_euler(*angles) return trans, rot
def d2q(self, X, rndFactor): X = self.d2r(X) q = t.quaternion_from_euler(X[3], X[4], X[5], 'sxyz') X[3] = round(q[0], rndFactor) X[4] = round(q[1], rndFactor) X[5] = round(q[2], rndFactor) X.append(round(q[3], rndFactor)) return X
def publish_true_path(self, pose, publish_odom): # count current coordinates and direction in global coords start_time = rospy.Time.now() position, rotation = pose y, z, x = position cur_orientation = rotation cur_euler_angles = tf.euler_from_quaternion([cur_orientation.w, cur_orientation.x, cur_orientation.z, cur_orientation.y]) cur_x_angle, cur_y_angle, cur_z_angle = cur_euler_angles cur_z_angle += np.pi print('Source position:', y, z, x) print('Source quat:', cur_orientation.x, cur_orientation.y, cur_orientation.z, cur_orientation.w) print('Euler angles:', cur_x_angle, cur_y_angle, cur_z_angle) #print('After tf:', tf.quaternion_from_euler(cur_x_angle, cur_y_angle, cur_z_angle)) if self.publish_odom: self.slam_update_time = start_time.secs + 1e-9 * start_time.nsecs if not self.is_started: self.is_started = True self.slam_start_angle = cur_z_angle print("SLAM START ANGLE:", self.slam_start_angle) self.slam_start_x = x self.slam_start_y = y self.slam_start_z = z # if SLAM is running, transform global coords to RViz coords if self.publish_odom or (start_time.secs + start_time.nsecs * 1e-9) - self.slam_update_time < 30: rviz_x, rviz_y = inverse_transform(x, y, self.slam_start_x, self.slam_start_y, self.slam_start_angle) rviz_z = z - self.slam_start_z cur_quaternion = tf.quaternion_from_euler(0, 0, cur_z_angle - self.slam_start_angle) print('Rotated quat:', cur_quaternion) cur_orientation.w = cur_quaternion[0] cur_orientation.x = cur_quaternion[1] cur_orientation.y = cur_quaternion[2] cur_orientation.z = cur_quaternion[3] x, y, z = rviz_x, rviz_y, rviz_z self.true_positions.append(np.array([x, y, z])) self.true_rotations.append(cur_quaternion) # add current point to path cur_pose = PoseStamped() cur_pose.header.stamp = start_time cur_pose.pose.position.x = x cur_pose.pose.position.y = y cur_pose.pose.position.z = z cur_pose.pose.orientation = cur_orientation self.true_trajectory.append(cur_pose) # publish the path true_path = Path() print(start_time) true_path.header.stamp = start_time true_path.header.frame_id = 'map' true_path.poses = self.true_trajectory self.true_path_publisher.publish(true_path) # publish odometry if self.publish_odom: odom = Odometry() odom.header.stamp = start_time odom.header.frame_id = 'odom' odom.child_frame_id = 'base_link' odom.pose.pose = cur_pose.pose self.odom_publisher.publish(odom)
def _extract_twist_msg(twist_msg): pos = [twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z] rot_euler = [ twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z ] quat = trans.quaternion_from_euler(*rot_euler, axes='sxyz') homo_mat = np.mat(trans.euler_matrix(*rot_euler)) homo_mat[:3, 3] = np.mat([pos]).T return homo_mat, quat, rot_euler
def theta(self, theta): assert len(theta) == 6 self._theta = theta for th, joint, offset, axes in zip( theta, self._joints, self._offsets, self._axes ): joint.quaternion = tuple( np.roll(quaternion_from_euler(*((th + offset) * axes)), -1) )
def rotate(self, roll, pitch, yaw): ''' Rotate the render object around the axes. Raises an error if self.orientation is None. ''' ori = self.orientation if ori is None: raise self.orientation = quaternion_multiply(quaternion_from_euler(roll, pitch, yaw), ori)
def flip_pelvis_coordinate_system(q): conv_m = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) m = quaternion_matrix(q) new_m = np.dot(conv_m, np.dot(m, conv_m)) q = quaternion_from_matrix(new_m) flip_q = quaternion_from_euler(*np.radians([0, 0, 180])) q = quaternion_multiply(flip_q, q) return q
def place_marker(self, id, marker): x = marker["x"] y = marker["y"] on_wall = marker["wall"] w = 0 translate = "translated" not in marker.keys or not marker["translated"] if on_wall == "north": if translate: y = y + 0.1 w = -math.pi / 2 elif on_wall == "south": if translate: y = y - 0.1 w = math.pi / 2 elif on_wall == "east": if translate: x = x - 0.125 w = math.pi elif on_wall == "west": if translate: x = x + 0.05 w = 0 else: print("Marker is not on a wall!?") # set up position of the obstacle gx, gy = self.translateMapToGazebo(x, y) pose = Pose() pose.position.x = gx pose.position.y = gy pose.position.z = 0.75 q = tf.quaternion_from_euler(0, 0, w) pose.orientation.x = q[0] pose.orientation.y = q[1] pose.orientation.z = q[2] pose.orientation.w = q[3] # Generate obstacles name (in threadsafe manner) # Create gazebo request and call req = SpawnModelRequest() req.model_name = "Marker%s" % id req.model_xml = MARKER_TEMPLATE.replace("$ID", str(id)) req.initial_pose = pose try: res = self.spawn_model(req) if res.success: return True else: rospy.logerr("Could not place obstacle. Message: %s" % res.status_message) return False except rospy.ServiceException as e: rospy.logerr("Could not place obstacle. Message %s" % e) return False
def flip_root_coordinate_system(q1): conv_m = np.array([[-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]) m = quaternion_matrix(q1) new_m = np.dot(conv_m, np.dot(m, conv_m)) q2 = quaternion_from_matrix(new_m) flip_q = quaternion_from_euler(*np.radians([0, 0, 180])) q2 = quaternion_multiply(flip_q, q2) return q2
def move_rotate(self,alfax,alfay,alfaz, ax, ay, az): if alfax <> 0: qr = transformations.quaternion_from_euler(0.0, -alfax, 0.0 ) self.input_orientation = transformations.quaternion_multiply(self.input_orientation, qr) #add code to renormalize the quat if alfay <> 0: qr = transformations.quaternion_from_euler( -alfay,0.0, 0.0) self.input_orientation = transformations.quaternion_multiply(self.input_orientation, qr) #add code to renormalize the quat if alfaz <> 0: qr = transformations.quaternion_from_euler( 0.0, 0.0, -alfaz) self.input_orientation = transformations.quaternion_multiply(self.input_orientation, qr) #add code to renormalize the quat self.c_acceleration[:]=[ax,ay,az]
def set_target_object(self, random_object=False, random_position=False): """ Set target object :param random_object: spawn object randomly from the object pool. If false, object will be the first entry of the object list :param random_position: spawn object with random position """ if random_object: rand_object = random.choice(self.object_list) self.object_name = rand_object["name"] self.object_type_str = rand_object["type"] self.object_type = self.object_list.index(rand_object) init_pos = rand_object["init_pos"] self.object_initial_position = Pose( position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]), orientation=quaternion_from_euler(init_pos[3], init_pos[4], init_pos[5])) else: self.object_name = self.object_list[0]["name"] self.object_type_str = self.object_list[0]["type"] self.object_type = 0 init_pos = self.object_list[0]["init_pos"] self.object_initial_position = Pose( position=Point(x=init_pos[0], y=init_pos[1], z=init_pos[2]), orientation=quaternion_from_euler(init_pos[3], init_pos[4], init_pos[5])) if random_position: if self.object_type_str == "door_handle": box_pos = U.get_random_door_handle_pos() else: box_pos = Pose(position=Point(x=np.random.uniform(low=-0.3, high=0.3, size=None), y=np.random.uniform(low=0.9, high=1.1, size=None), z=1.05), orientation=quaternion_from_euler(0, 0, 0)) else: box_pos = self.object_initial_position U.change_object_position(self.object_name, box_pos) print("Current target: ", self.object_name)
def quaternion_from_vector_to_vector(a, b): """src: http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another""" if np.all(a == b): return [1,0,0,0] v = np.cross(a, b) if np.linalg.norm(v) == 0.0: return quaternion_from_euler(*np.radians([180,0,0])) w = np.sqrt((np.linalg.norm(a) ** 2) * (np.linalg.norm(b) ** 2)) + np.dot(a, b) q = np.array([w, v[0], v[1], v[2]]) return q / np.linalg.norm(q)
def _process_vertical_axis(self, euler_angles, axes): if axes[1] == self._vertical_axis: return self._process_vertical_orientation_as_first_axis(euler_angles) else: if self._vertical_axis == "y": axes_with_vertical_first = "ryxz" elif self._vertical_axis == "z": axes_with_vertical_first = "rzxy" euler_angles_vertical_axis_first = euler_from_quaternion( quaternion_from_euler(*euler_angles, axes=axes), axes=axes_with_vertical_first) euler_angles_vertical_axis_first_reoriented = \ self._process_vertical_orientation_as_first_axis( euler_angles_vertical_axis_first) return euler_from_quaternion( quaternion_from_euler( *euler_angles_vertical_axis_first_reoriented, axes=axes_with_vertical_first), axes=axes)
def computeCameraPoseFromAircraft(image, cam, ref, yaw_bias=0.0, roll_bias=0.0, pitch_bias=0.0, alt_bias=0.0): lla, ypr, ned2body = image.get_aircraft_pose() aircraft_lat, aircraft_lon, aircraft_alt = lla #print "aircraft quat =", world2body msl = aircraft_alt + image.alt_bias + alt_bias (camera_yaw, camera_pitch, camera_roll) = cam.get_mount_params() body2cam = transformations.quaternion_from_euler(camera_yaw * d2r, camera_pitch * d2r, camera_roll * d2r, 'rzyx') ned2cam = transformations.quaternion_multiply(ned2body, body2cam) (yaw, pitch, roll) = transformations.euler_from_quaternion(ned2cam, 'rzyx') ned = navpy.lla2ned( aircraft_lat, aircraft_lon, aircraft_alt, ref[0], ref[1], ref[2] ) #print "aircraft=%s ref=%s ned=%s" % (image.get_aircraft_pose(), ref, ned) return (ned.tolist(), [yaw/d2r, pitch/d2r, roll/d2r])
def sendSetpoint(): global run setPointsCount = 0 local_setpoint_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=1) rate = rospy.Rate(20) while run: q = quaternion_from_euler(0, 0, 0, axes="sxyz") msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.header.seq=setPointsCount msg.pose.position.x = 0 msg.pose.position.y = 0 msg.pose.position.z = 0 msg.pose.orientation.x = q[0] msg.pose.orientation.y = q[1] msg.pose.orientation.z = q[2] msg.pose.orientation.w = q[3] local_setpoint_pub.publish(msg) setPointsCount = setPointsCount + 1 rate.sleep()
def __init__(self, T): if ( type(T) == str): Tfile = T #Load transformation Tfis = open(Tfile, 'r') lines = [] lines = Tfis.readlines() self.scale = float(lines[0]) self.Ss = tf.scale_matrix(self.scale) quat_line = lines[1].split(" ") self.quat = tf.unit_vector(np.array([float(quat_line[3]), float(quat_line[0]), float(quat_line[1]), float(quat_line[2])])) self.Hs = tf.quaternion_matrix(self.quat) trans_line = lines[2].split(" ") self.Ts = np.array([float(trans_line[0]), float(trans_line[1]), float(trans_line[2])]) Tfis.close() self.Rs = self.Hs.copy()[:3, :3] self.Hs[:3, 3] = self.Ts[:3] self.Hs = self.Ss.dot(self.Hs) # to add again elif (type(T) == np.ndarray): self.Hs = T scale, shear, angles, trans, persp = tf.decompose_matrix(T) self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2]) self.Rs = tf.quaternion_matrix(self.quat) self.scale =scale[0] self.Ts = trans / self.scale print "Loaded Ground Truth Transformation: " print self.Hs
def __init__(self, T): if ( type(T) == str): print "Loading Transformation from file: " + T Tfile = T #Load transformation Tfis = open(Tfile, 'r') lines = [] lines = Tfis.readlines() format = len(lines) Tfis.seek(0) #reset file pointer if not (format==3 or format==4 or format==5) : raise ValueError("Wrong number of lines in file") # import code; code.interact(local=locals()) if format == 3: """Handles processing a ground truth transfomation File saved using vxl format x' = s *(Rx + T) scale quaternion translation """ print("Reading format 3") self.scale = float(lines[0]) self.Ss = tf.scale_matrix(self.scale) quat_line = lines[1].split(" ") self.quat = tf.unit_vector(np.array([float(quat_line[3]), float(quat_line[0]), float(quat_line[1]), float(quat_line[2])])) self.Hs = tf.quaternion_matrix(self.quat) trans_line = lines[2].split(" ") self.Ts = np.array([float(trans_line[0]), float(trans_line[1]), float(trans_line[2])]) Tfis.close() self.Rs = self.Hs.copy()[:3, :3] self.Hs[:3, 3] = self.Ts[:3] self.Hs = self.Ss.dot(self.Hs) # to add again if format == 4 : """If the transformation was saved as: H (4x4) - = [S*R|S*T] """ print("Reading format 4") self.Hs = np.genfromtxt(Tfile, usecols={0, 1, 2, 3}) Tfis.close() scale, shear, angles, trans, persp = tf.decompose_matrix(self.Hs) self.scale = scale[0] # assuming isotropic scaling self.Rs = self.Hs[:3, :3] * (1.0 / self.scale) self.Ts = self.Hs[:3, 3] * (1.0 / self.scale) self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2]) if format==5: """If the transformation was saved as: scale H (4x4) - = [S*R|S*T] """ print("Reading format 5") self.Hs = np.genfromtxt(Tfis, skip_header=1, usecols={0, 1, 2, 3}) Tfis.close() Tfis = open(Tfile, 'r') self.scale = np.genfromtxt(Tfis, skip_footer=4, usecols={0}) Tfis.close() self.Rs = self.Hs[:3, :3] * (1.0 / self.scale) self.Ts = self.Hs[:3, 3] * (1.0 / self.scale) scale, shear, angles, trans, persp = tf.decompose_matrix(self.Hs) self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2]) print "Debugging translation:" print self.Ts print trans/self.scale elif (type(T) == np.ndarray): print "Loading Transformation array" self.Hs = T scale, shear, angles, trans, persp = tf.decompose_matrix(T) self.scale =scale[0] self.Rs = self.Hs[:3, :3] * (1.0 / self.scale) self.Ts = self.Hs[:3, 3] * (1.0 / self.scale) self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2]) print "Debugging translation:" print self.Ts print trans/self.scale # self.Rs = tf.quaternion_matrix(self.quat) # self.Ts = trans / self.scale print self.Hs
def ros_publisher(self): global odometry_x_ global odometry_y_ global odometry_yaw_ global right_encoder_prev global left_encoder_prev global current_time global data_publish_flag ser.write('S') print ser.inWaiting() ser_data='' if ser.inWaiting()==27: ser_data=ser.read(27) data="hello" #print type(data), len(data) #print type(ser_data), len(ser_data) encoder1_data=ord(ser_data[2])*255+ord(ser_data[3])*255+ord(ser_data[4])*255+ord(ser_data[5]) encoder2_data=ord(ser_data[6])*255+ord(ser_data[7])*255+ord(ser_data[8])*255+ord(ser_data[9]) #print 'raw' #print encoder1_data #print encoder2_data encoder1='0x{0:08X}.format(encoder1_data)' encoder2='0x{0:08X}.format(encoder2_data)' signed_encoder1=~(0xffffffff - int(encoder1,16))+1 signed_encoder2=~(0xffffffff - int(encoder2,16))+1 print 'raw' print signed_encoder1 print signed_encoder2 msg1 = {'op': 'publish', 'topic': '/encoder1', 'msg':{'data' : encoder1_data}} msg2 = {'op': 'publish', 'topic': '/encoder2', 'msg':{'data' : encoder2_data}} msg3 = {'op': 'publish', 'topic': '/sonar1', 'msg':{'range': ord(ser_data[10])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar1'}}} msg4 = {'op': 'publish', 'topic': '/sonar2', 'msg':{'range': ord(ser_data[11])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar'}}} msg5 = {'op': 'publish', 'topic': '/sonar3', 'msg':{'range': ord(ser_data[12])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar3'}}} msg6 = {'op': 'publish', 'topic': '/sonar4', 'msg':{'range': ord(ser_data[13])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar4'}}} msg7 = {'op': 'publish', 'topic': '/sonar5', 'msg':{'range': ord(ser_data[14])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar5'}}} msg8 = {'op': 'publish', 'topic': '/sonar6', 'msg':{'range': ord(ser_data[15])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar6'}}} msg9 = {'op': 'publish', 'topic': '/sonar7', 'msg':{'range': ord(ser_data[16])/10.0, 'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar7'}}} msg10= {'op': 'publish', 'topic': '/sonar8', 'msg':{'range': ord(ser_data[17])/10.0,'radiation_type' : 0, 'field_of_view' : 0.1, 'min_range':0.05, 'max_range': 2.00,'header':{'frame_id':'sonar8'}}} self.send(dumps(msg1)) self.send(dumps(msg2)) self.send(dumps(msg3)) self.send(dumps(msg4)) self.send(dumps(msg5)) self.send(dumps(msg6)) self.send(dumps(msg7)) self.send(dumps(msg8)) self.send(dumps(msg9)) self.send(dumps(msg10)) last_x=odometry_x_ last_y=odometry_y_ last_yaw=odometry_yaw_ right_enc_dif=(encoder1_data-right_encoder_prev)*0.0008888 left_enc_dif=(encoder2_data-left_encoder_prev)*0.0008888 #print 'enc dif' #print right_encoder_prev #print encoder1_data #print left_encoder_prev #print encoder2_data #print right_enc_dif #print left_enc_dif #calculate odometry dist=(right_enc_dif+left_enc_dif)/2.0 ang=(right_enc_dif-left_enc_dif)/0.28 #print 'ang' #print ang #print dist right_encoder_prev=encoder1_data left_encoder_prev=encoder2_data odometry_yaw_ = math.atan2(math.sin(odometry_yaw_+ang), math.cos(odometry_yaw_+ang)) odometry_x_=odometry_x_+dist*math.cos(odometry_yaw_) odometry_y_=odometry_y_+dist*math.sin(odometry_yaw_) #print 'odom' #print odometry_x_ #print odometry_y_ #print odometry_yaw_ last_time=current_time current_time=time.time() #print current_time #print 'odom' dt=(current_time-last_time) vel=dist/dt vel_x=(odometry_x_-last_x)/dt vel_y=(odometry_y_-last_y)/dt vel_yaw=(odometry_yaw_-last_yaw)/dt #print 'time' #print (odometry_x_ - last_x) #print dt #print 'twist msgs' #print vel #print vel_x #print vel_yaw orient=tf.quaternion_from_euler(0,0,odometry_yaw_) msg11= {'op': 'publish', 'topic': '/odom', 'msg': {'pose': {'pose':{'position':{'x':odometry_x_, 'y':odometry_y_, 'z':0.0},'orientation': {'x':orient.item(1), 'y':orient.item(2), 'z':orient.item(3), 'w':orient.item(0) }}},'child_frame_id': 'base_link','twist' : {'twist':{'linear':{'x':vel_x, 'y':vel_y }, 'angular':{'z':vel_yaw} }},'header':{'frame_id':'odom'}}} self.send(dumps(msg11)) I=transformations.identity_matrix() #print I else: ser.flushInput() #data="hello" #print type(data), len(data) #print type(ser_data), len(ser_data) #msg = {'op': 'publish', 'topic': '/rosbridge_example', 'msg':{'data' : data}} #self.send(dumps(msg)) threading.Timer(0.08,self.ros_publisher).start()
def orientation_from_euler(self, roll, pitch, yaw): if self.orientation is None: raise NotImplemented("This RenderObject has no orientation") self.orientation = quaternion_from_euler(roll, pitch, yaw)
plotter = rpp.Plotter() cubes = [] for i in range(0,N): # Generate a random cube scale = np.random.rand(3,1)*1.0 p = np.random.rand(3,1)*5.0 euler = np.random.rand(3,1) euler[1] = 0.0 euler[2] = 0.0 print "yaw =",euler[0] quat = tr.quaternion_from_euler(euler[0], euler[1], euler[2]) pose=(p,quat) # Generate a random color color = np.random.rand(4) color[3] = 0.5 # the color alpha # The color is optional cubeMarker = rpp.CubeMarker(pose, scale=scale, color=color, markerId=i) cubes.append(cubeMarker) # Plot the lines plotter.plot(cubes)
pilot_thr = float(flight_pilot_thr(time)) pilot_rud = float(flight_pilot_rud(time)) auto_switch = float(flight_pilot_auto(time)) act_ail = float(flight_act_ail(time)) act_ele = float(flight_act_ele(time)) act_thr = float(flight_act_thr(time)) act_rud = float(flight_act_rud(time)) if args.auto_switch == 'none': flight_mode = 'manual' elif (args.auto_switch == 'new' and auto_switch < 0) or (args.auto_switch == 'old' and auto_switch > 0): flight_mode = 'manual' else: flight_mode = 'auto' body2cam = transformations.quaternion_from_euler( cam_yaw * d2r, cam_pitch * d2r, cam_roll * d2r, 'rzyx') #print 'att:', [yaw_rad, pitch_rad, roll_rad] ned2body = transformations.quaternion_from_euler(yaw_rad, pitch_rad, roll_rad, 'rzyx') body2ned = transformations.quaternion_inverse(ned2body) #print 'ned2body(q):', ned2body ned2cam_q = transformations.quaternion_multiply(ned2body, body2cam) ned2cam = np.matrix(transformations.quaternion_matrix(np.array(ned2cam_q))[:3,:3]).T #print 'ned2cam:', ned2cam R = ned2proj.dot( ned2cam ) rvec, jac = cv2.Rodrigues(R)
def quaternion_from_euler(rx, ry, rz): return transformations.quaternion_from_euler(rx, ry, rz, 'sxyz')
pitch = np.delete(pitch,0) roll = np.delete(roll,0) time = np.delete(time,0) p = rollRate - yawRate * np.sin(pitch) q = pitchRate * np.cos(roll) + yawRate * np.sin(roll) * np.cos(pitch) r = -pitchRate * np.sin(roll) + yawRate * np.cos(roll) * np.cos(pitch) p = p + sigma * np.random.randn(len(p)) + bias[0] q = q + sigma * np.random.randn(len(q)) + bias[1] r = r + sigma * np.random.randn(len(r)) + bias[2] q_est = trans.quaternion_from_euler(0,0,0) biasEst = np.array([0,0,0]) q_insOnly = trans.quaternion_from_euler(0,0,0) gain = 0.001 bgain = 0.001 yawEst = [] yawINS = [] pitchEst = [] pitchINS = [] rollEst = [] rollINS = [] yaw_meas = []
def rpypose_to_quatpose(trans, rpy): """Returns the rpy pose from a quaternion pose.""" quat = tf.quaternion_from_euler(*rpy, axes='sxyz') return np.array(trans), quat
def rotation_to_parameters(euler): return quaternion_from_euler(*euler.angles, axes=euler.axes)
rpy_filt[:,0] = signal.filtfilt(b, a, rpy[:,0]) rpy_filt[:,1] = signal.filtfilt(b, a, rpy[:,1]) rpy_filt[:,2] = signal.filtfilt(b, a, rpy[:,2]) fig = plt.figure() ax = fig.add_subplot(111, title='orientation filtered') ax.plot(rpy[:,0], 'r-') ax.plot(rpy[:,1], 'g-') ax.plot(rpy[:,2], 'b-') ax.plot(rpy_filt[:,0], 'k-', linewidth=2) ax.plot(rpy_filt[:,1], 'k-', linewidth=2) ax.plot(rpy_filt[:,2], 'k-', linewidth=2) fig = plt.figure() ax = fig.add_subplot(111, title='position') ax.plot(D[:,1], 'r') ax.plot(D[:,2], 'g') ax.plot(D[:,3], 'b') fig = plt.figure() ax = fig.add_subplot(111, title='trajectory from top') ax.plot(D[:,1], D[:,2]) if save: f = open(filtered_data_filename,'w') for i in range(np.shape(D)[0]): quat = transformations.quaternion_from_euler(rpy_filt[i,0], rpy_filt[i,1], rpy_filt[i,2], axes='sxyz') f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (D[i,0], D[i,1], D[i,2], D[i,3], quat[0], quat[1], quat[2], quat[3])) f.close()
def yaw_to_quat(yaw): return transformations.quaternion_from_euler(0, 0, yaw)
def set_aircraft_pose(self, lla=[0.0, 0.0, 0.0], ypr=[0.0, 0.0, 0.0]): quat = transformations.quaternion_from_euler(ypr[0] * d2r, ypr[1] * d2r, ypr[2] * d2r, "rzyx") self.aircraft_pose = {"lla": lla, "ypr": ypr, "quat": quat.tolist()}
def makeMotion(ip,port,lock): useSensors = True if ip == '127.0.0.1': useSensors = False motion = getMotionProxy(ip,port) posture = getPostureProxy(ip,port) memory = getMemoryProxy(ip,port) moveInit(motion,posture) #ret = action_moveTo(motion,posture,1.2,0,math.pi/2) ret = action_moveTo(motion,posture,0.75,0,0) #ret = action_moveTo(motion,posture,0.75,0,0) #global gPose while ret[1].isRunning(ret[0]): # motion.FRAME_WORLD odomData = motion.getPosition("Torso",1, True) memData = memory.getListData(dataNamesList) #print odomData #positionData = motion.getAngles("Body", True) #print positionData pos = [odomData[0], odomData[1], odomData[2]] rot = transformations.quaternion_from_euler(odomData[3], odomData[4], odomData[5]) rPos = motion.getRobotPosition(useSensors) rNextPos = motion.getNextRobotPosition() rVel = motion.getRobotVelocity() #rot2 = transformations.quaternion_from_euler(memData[1], memData[2], memData[3]) lock.acquire() local = gPose localFiltered = gPoseFiltered lock.release() if local != None: print "O:", pos[0],pos[1],pos[2], rot[0],rot[1],rot[2],rot[3], getHeading(rot), 0, 0 print "L:", local[0]/1000,local[1]/1000,local[2]/1000, local[3],local[4],local[5],local[6], local[7], 0, 0 print "L_F:", localFiltered[0]/1000,localFiltered[1]/1000,localFiltered[2]/1000, localFiltered[3],localFiltered[4],localFiltered[5],localFiltered[6], localFiltered[7], 0, 0 print "IMU:", memData print "LOC:", rPos[0],rPos[1],rPos[2],rNextPos[0],rNextPos[1],rNextPos[2],rVel[0],rVel[1],rVel[2],0 #print gPose time.sleep(0.100) for i in range(1,10): odomData = motion.getPosition("Torso",1, True) memData = memory.getListData(dataNamesList) #print odomData #positionData = motion.getAngles("Body", True) #print positionData pos = [odomData[0], odomData[1], odomData[2]] rot = transformations.quaternion_from_euler(odomData[3], odomData[4], odomData[5]) #rot2 = transformations.quaternion_from_euler(memData[1], memData[2], memData[3]) rPos = motion.getRobotPosition(useSensors) rNextPos = motion.getNextRobotPosition() rVel = motion.getRobotVelocity() lock.acquire() local = gPose localFiltered = gPoseFiltered lock.release() if local != None: print "O:", pos[0],pos[1],pos[2], rot[0],rot[1],rot[2],rot[3], getHeading(rot), 0, 0 print "L:", local[0]/1000,local[1]/1000,local[2]/1000, local[3],local[4],local[5],local[6], local[7], 0, 0 print "L_F:", localFiltered[0]/1000,localFiltered[1]/1000,localFiltered[2]/1000, localFiltered[3],localFiltered[4],localFiltered[5],localFiltered[6], localFiltered[7], 0, 0 print "IMU:", memData print "LOC:", rPos[0],rPos[1],rPos[2],rNextPos[0],rNextPos[1],rNextPos[2],rVel[0],rVel[1],rVel[2],0 #print gPose time.sleep(0.100) time.sleep(3) global stop lock.acquire() stop = True lock.release()
def set_camera_pose_sba(self, ned=[0.0, 0.0, 0.0], ypr=[0.0, 0.0, 0.0]): quat = transformations.quaternion_from_euler(ypr[0] * d2r, ypr[1] * d2r, ypr[2] * d2r, "rzyx") self.camera_pose_sba = {"ned": ned, "ypr": ypr, "quat": quat.tolist()}
import os import sys workspace_dir = os.path.dirname(os.path.abspath(__file__)) + '/..' output_dir = os.path.join(workspace_dir, 'output') traj_euler = np.loadtxt(os.path.join(output_dir, 'txt', "traj_euler.txt")) output_file = os.path.join(output_dir, 'txt', "traj_quaternion.txt") open(output_file, 'w').close traj_quaternion = open(output_file, 'w') for row in traj_euler: # convert to z-upward coordinate system # only need to transform the position #Rz = tf.rotation_matrix(pi/2, [0, 0, 1]) #Rx = tf.rotation_matrix(-pi, [1, 0, 0]) #R = tf.concatenate_matrices(Rz, Rx) #RT = np.transpose(R) #R_org = tf.euler_matrix(row[4], row[5], row[6], 'rzyx') #R_total = tf.concatenate_matrices(R, R_org) #R_total = tf.concatenate_matrices(R_total, RT) row[3] = -row[3] row[1], row[2] = row[2], row[1] # convert to quaternion # note: the quaternion order as [x y z w] # from downward-z to upward-z and x-y swap # rotz(yaw)*rosy(pitch)*rotx(roll) -> rotz(-yaw)*rotx(pitch)*roty(roll) quaternion = tf.quaternion_from_euler((-1)*row[4], row[5], row[6], 'rzxy'); traj_quaternion.write("%.6f %.4f %.4f %.4f %.4f %.4f %.4f %.4f\n" %(row[0], row[1], row[2], row[3], quaternion[1], quaternion[2], quaternion[3], quaternion[0]));
marker.pose.pose.orientation.w, ) quats[str(marker.id)] = [ marker.pose.pose.orientation.x, marker.pose.pose.orientation.y, marker.pose.pose.orientation.z, marker.pose.pose.orientation.w, ] mean_poses = {} for m in engine: print "-------------", m, "------------------" print len(engine[m]) print np.mean(engine[m], axis=0) # , np.var(engine[m], axis=0) maxM = max(engine[m]) minM = min(engine[m]) print [maxM[0] - minM[0], maxM[1] - minM[1], maxM[2] - minM[2]] mean_poses[m] = np.mean(engine[m], axis=0) # q = [quats[m][0], quats[m][1], quats[m][2], quats[m][3]] pos = mean_poses[m][0:3] q = transformations.quaternion_from_euler(mean_poses[m][3], mean_poses[m][4], mean_poses[m][5]) mean_poses[m] = [pos[0], pos[1], pos[2], q[0], q[1], q[2], q[3]] print mean_poses yaml.dump(mean_poses, file(str("consolidate_model.yaml"), "w"))