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"))
