def init_move(robot, cam):
global dL, dx, dy, dz, dax, day, daz
global angles_current, angles_previous
global L2, x2, y2, z2, qtx2, qty2, qtz2, qtw2
global joint_current, joint_previous
joint_current = np.array(list(robot.last_joint_pose))
L = 0.3 # rong: read from image
x, y, z = robot.last_tool_pose.position.x, robot.last_tool_pose.position.y, robot.last_tool_pose.position.z
qtx, qty, qtz, qtw = robot.last_tool_pose.orientation.x, robot.last_tool_pose.orientation.y, robot.last_tool_pose.orientation.z, robot.last_tool_pose.orientation.w
anglex, angley, anglez = euler_from_quaternion([qtw, -qtx, -qty, -qtz])
joint_previous = joint_current
joint_current = joint_previous + np.array(
[.002, .002, .002, .002, .002, .002, .002])
#print new_joint
robot.joint_move(joint_current)
# after a shortmovement, read cartesian values
L2 = L - 0.005 # rong: read from image
x2, y2, z2 = robot.last_tool_pose.position.x, robot.last_tool_pose.position.y, robot.last_tool_pose.position.z
qtx2, qty2, qtz2, qtw2 = robot.last_tool_pose.orientation.x, robot.last_tool_pose.orientation.y, robot.last_tool_pose.orientation.z, robot.last_tool_pose.orientation.w
anglex2, angley2, anglez2 = euler_from_quaternion(
[qtw2, -qtx2, -qty2, -qtz2])
dL = L2 - L
dx, dy, dz, dax, day, daz = x2 - x, y2 - y, z2 - z, anglex2 - anglex, angley2 - angley, anglez2 - anglez
angles_current = np.array([anglex2, angley2, anglez2])
print '##### initial move done ########'
print dL, dx, dy, dz, dax, day, daz
def ImuCallback(self, msg): """Handle IMU message""" if self.imu_last_update_time: # Convert to numpy self.lin_acc = np.array([ msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z ]) self.ang_vel = np.array([ msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z ]) ############################################################################# # Setup map orientation array euler = euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]) # Convert IMU data from quarternion to euler unwrapEuler = np.unwrap(euler) # unwrap euler angles imu_mapEulAng = np.array([[unwrapEuler[0]], [unwrapEuler[1]], [unwrapEuler[2]]]) # imu angular position array ### NOTES: Incoming IMU data is in rotation matrix form ### self.imu_mapAngPos = Rot(imu_mapEulAng[0], imu_mapEulAng[1], imu_mapEulAng[2]) ############################################################################# dt = msg.header.stamp.to_sec() - self.imu_last_update_time if self.fixed_dt: dt = self.fixed_dt # IMU update imu_pos, imu_ori, vel, acc_bias, gyr_bias = self.__fusion_core.AddImuMeasurement( msg.header.stamp.to_sec(), self.lin_acc, self.ang_vel, dt) # convert pose from IMU frame to robot frame rbt_pos = imu_pos rbt_ori = imu_ori # store internally euler_imu_ori = np.asarray(euler_from_quaternion(imu_ori)) / np.pi * 180. self.last_rbt_pose = (rbt_pos, rbt_ori) self.fgo_pose = np.concatenate((imu_pos, euler_imu_ori, vel), axis=0) # publish pose self.__publish_pose( msg.header.stamp, self.map_frame, rbt_pos, rbt_ori ) # data for plots if self.plot_results: # store input self.results['IMU'].append( np.concatenate((np.array([msg.header.stamp.to_sec(), dt]), self.lin_acc, self.ang_vel), axis=0)) # store output self.results[self.fusion_items].append( np.concatenate((np.array([msg.header.stamp.to_sec()]), imu_pos, euler_imu_ori, vel, acc_bias, gyr_bias), axis=0)) self.imu_last_update_time = msg.header.stamp.to_sec()
def jac_rpy_qut_numerical_tfs(q):
J = np.zeros((3, 4))
eps = 1e-6
q_ = copy.copy(q)
rpy0 = np.array(tfs.euler_from_quaternion(q))
for i in range(4):
q_[i] += eps
rpy1 = np.array(tfs.euler_from_quaternion(q_))
hoge = (rpy1 - rpy0)/eps
J[:, i] = hoge
return J
def listener():
tf_listener = tf.TransformListener()
global MSG, MSGR1, MSGR2, i
while not rospy.is_shutdown():
try:
now = rospy.Time.now()
(trans, rot) = tf_listener.lookupTransform("/pioneer1/odom",
"/map", rospy.Time(0))
(roll, pitch, yaw) = euler_from_quaternion(rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "tf error"
try:
now = rospy.Time.now()
(trans, rot) = tf_listener.lookupTransform("/pioneer2/odom",
"/map", rospy.Time(0))
(roll, pitch, yaw) = euler_from_quaternion(rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "tf error"
#MSGR1=np.resize(MSGR1,[i+1,len(MSG[0][:])])
MSGR1[i][:] = np.array(MSG[0][:])
#MSGR2=np.resize(MSGR2,[i+1,len(MSG[1][:])])
print i
MSGR2[i][:] = np.array(MSG[1][:])
#obj_arr = np.zeros((4,), dtype=np.object)
#obj_arr[0] = 'Rob 1 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
#obj_arr[1] = MSGR1
#obj_arr[2] = 'Rob 2 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
#obj_arr[3] = MSGR2
#scipy.io.savemat('np_cells.mat', {'obj_arr':obj_arr})
#f.write('{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}'.format(MSG[rob][0],MSG[rob][1],MSG[rob][2],MSG[rob][3],MSG[rob][4],MSG[rob][5],MSG[rob][6],MSG[rob][7],MSG[rob][8],MSG[rob][9],MSG[rob][10],MSG[rob][11],MSG[rob][12],MSG[rob][13],MSG[rob][14],MSG[rob][15],MSG[rob][16],MSG[rob][17],servoang1))
#rob=1
#f.write(',{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}'.format(MSG[rob][0],MSG[rob][1],MSG[rob][2],MSG[rob][3],MSG[rob][4],MSG[rob][5],MSG[rob][6],MSG[rob][7],MSG[rob][8],MSG[rob][9],MSG[rob][10],MSG[rob][11],MSG[rob][12],MSG[rob][13],MSG[rob][14],MSG[rob][15],MSG[rob][16],MSG[rob][17],servoang2))
#f.write('\n')
#MSG[0][:]=np.zeros(18)
#MSG[1][:]=np.zeros(18)
i = i + 1
if i == 6000:
obj_arr = np.zeros((4, ), dtype=np.object)
obj_arr[0] = 'Rob 1 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
#print MSGR1
obj_arr[1] = MSGR1
obj_arr[2] = 'Rob 2 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
obj_arr[3] = MSGR2
scipy.io.savemat('np_cells.mat', {'obj_arr': obj_arr})
rate.sleep()
def listener():
tf_listener = tf.TransformListener()
global MSG, MSGR1, MSGR2, i
while not rospy.is_shutdown():
try:
now = rospy.Time.now()
(trans,rot) = tf_listener.lookupTransform("/pioneer1/odom", "/map", rospy.Time(0))
(roll,pitch,yaw) = euler_from_quaternion(rot)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "tf error"
try:
now = rospy.Time.now()
(trans,rot) = tf_listener.lookupTransform("/pioneer2/odom", "/map", rospy.Time(0))
(roll,pitch,yaw) = euler_from_quaternion(rot)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "tf error"
#MSGR1=np.resize(MSGR1,[i+1,len(MSG[0][:])])
MSGR1[i][:]=np.array(MSG[0][:])
#MSGR2=np.resize(MSGR2,[i+1,len(MSG[1][:])])
print i
MSGR2[i][:]=np.array(MSG[1][:])
#obj_arr = np.zeros((4,), dtype=np.object)
#obj_arr[0] = 'Rob 1 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
#obj_arr[1] = MSGR1
#obj_arr[2] = 'Rob 2 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
#obj_arr[3] = MSGR2
#scipy.io.savemat('np_cells.mat', {'obj_arr':obj_arr})
#f.write('{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}'.format(MSG[rob][0],MSG[rob][1],MSG[rob][2],MSG[rob][3],MSG[rob][4],MSG[rob][5],MSG[rob][6],MSG[rob][7],MSG[rob][8],MSG[rob][9],MSG[rob][10],MSG[rob][11],MSG[rob][12],MSG[rob][13],MSG[rob][14],MSG[rob][15],MSG[rob][16],MSG[rob][17],servoang1))
#rob=1
#f.write(',{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15},{16},{17},{18}'.format(MSG[rob][0],MSG[rob][1],MSG[rob][2],MSG[rob][3],MSG[rob][4],MSG[rob][5],MSG[rob][6],MSG[rob][7],MSG[rob][8],MSG[rob][9],MSG[rob][10],MSG[rob][11],MSG[rob][12],MSG[rob][13],MSG[rob][14],MSG[rob][15],MSG[rob][16],MSG[rob][17],servoang2))
#f.write('\n')
#MSG[0][:]=np.zeros(18)
#MSG[1][:]=np.zeros(18)
i=i+1
if i==6000:
obj_arr = np.zeros((4,), dtype=np.object)
obj_arr[0] = 'Rob 1 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
#print MSGR1
obj_arr[1] = MSGR1
obj_arr[2] = 'Rob 2 Header-x,y,z,theta,vx,omega,secs,nsecs,laser'
obj_arr[3] = MSGR2
scipy.io.savemat('np_cells.mat', {'obj_arr':obj_arr})
rate.sleep()
def leu_imu(dado):
global angulo
global crash
global bateu
global media
global diff
global i
# print(bateu)
quat = dado.orientation
lista = [quat.x, quat.y, quat.z, quat.w]
angulos = np.degrees(transformations.euler_from_quaternion(lista))
if len(crash) < 5:
crash.append(dado.linear_acceleration.x)
else:
crash[i] = dado.linear_acceleration.x
#print(crash)
i += 1
if i == 4:
i = 0
angulo = math.degrees(
math.atan2(dado.linear_acceleration.x, dado.linear_acceleration.y))
media = np.mean(crash)
diff = abs(crash[-1] - media)
if diff >= 3.5:
bateu = True
def imu_callback(self, data):
t = data.header.stamp.to_time()
if self.t0 == 0:
self.t0 = t
t = t - self.t0
gyro = np.array((data.angular_velocity.x, data.angular_velocity.y,
data.angular_velocity.z),
dtype=np.float32)
acc = np.array((data.linear_acceleration.x, data.linear_acceleration.y,
data.linear_acceleration.z),
dtype=np.float32)
imu_data = np.array(
(t, gyro[0], gyro[1], gyro[2], acc[0], acc[1], acc[2]))
self.estimator.predict(imu_data)
pos = self.estimator.nominal_state[:3]
quat = self.estimator.nominal_state[3:7]
euler = tr.euler_from_quaternion(quat)
self.pose_buf.append(self.estimator.nominal_state[1:7].copy())
if t > 10:
pose_data = np.array(self.pose_buf, dtype=np.float32)
np.savez('tmp/pose.npz', pose_data=pose_data)
rospy.signal_shutdown(0)
print('%.2f |' % t, '%6.2f %6.2f %6.2f |' % (pos[0], pos[1], pos[2]),
'%6.2f %6.2f %6.2f' % (euler[0], euler[1], euler[2]))
def hover(self):
with robotLock:
self.R.set_speed(speed=[10, 10, 50, 50])
with print_lock:
print "Speeding up to 10% 'hover'"
self.printDateTime()
print >> self.f, "Heading to Z coordinate %.1fmm" % self.HoverPosition[0][2],
self.f.flush()
while (self.R.get_cartesian() != self.HoverPosition):
self.R.set_cartesian(self.HoverPosition)
time.sleep(1)
self.R.set_speed(speed=[0.5, 0.5, 50, 50])
with print_lock:
print "Slowing Down to 1% 'hover'"
cartesian = self.R.get_cartesian()
euler = self.r2d(list(t.euler_from_quaternion(cartesian[1])))
euler = [round(euler[i], 1) for i in range(len(euler))]
cartesian[1] = euler
self.printDateTime()
print >> self.f, "Hovering at: ", cartesian
self.f.flush()
time.sleep(0.2)
saveImgEvent.clear()
if not saveImgEvent.wait(1):
print "Camera not working"
def dr_err_callback(self, gt_msg, dr_msg):
print('COMPUTE DR')
# Change incoming ground truth quarternion data into euler [rad]
gt_quaternion = (gt_msg.pose.pose.orientation.x,
gt_msg.pose.pose.orientation.y,
gt_msg.pose.pose.orientation.z,
gt_msg.pose.pose.orientation.w)
gt_euler = np.unwrap(euler_from_quaternion(gt_quaternion))
# # Change incoming dead reckoning quarternion data into euler [rad]
# dr_quaternion = (dr_msg.pose.pose.orientation.x, dr_msg.pose.pose.orientation.y, dr_msg.pose.pose.orientation.z, dr_msg.pose.pose.orientation.w)
# dr_euler = euler_from_quaternion(dr_quaternion)
# Instantiate arrays and variables
self.gt_pose = array([[gt_msg.pose.pose.position.x],
[gt_msg.pose.pose.position.y],
[gt_msg.pose.pose.position.z],
[np.rad2deg(gt_euler[0])],
[np.rad2deg(gt_euler[1])],
[np.rad2deg(gt_euler[2])],
[gt_msg.twist.twist.linear.x],
[gt_msg.twist.twist.linear.y],
[gt_msg.twist.twist.linear.z]
]) # ground truth array
self.dr_pose = array([[dr_msg.state.x], [dr_msg.state.y],
[dr_msg.state.z], [dr_msg.state.roll],
[dr_msg.state.pitch], [dr_msg.state.yaw],
[dr_msg.state.vx], [dr_msg.state.vy],
[dr_msg.state.vz]]) # estimator array
self.dr_dist_traveled, self.dr_dist_error_rmse = Err.DrAnalysis(
self.gt_pose, self.dr_pose)
# Publish
self.PublishDr()
def _process_root_orientation(self):
vertical_orientation = euler_from_quaternion(
self.get_joint("torso").get_orientation(), axes="rxyz")[1]
clamped_vertical_orientation = self._root_orientation_clamper.clamp(
vertical_orientation)
self._root_orientation_smoother.update(clamped_vertical_orientation)
self._smoothed_root_vertical_orientation = self._root_orientation_smoother.get_value()
def vtk_records_to_points(records):
""" Takes an array of VTK Records and returns an array of Points"""
points = []
for r in records:
if r.HasField('trackpoint'):
tp = r.trackpoint
p = {}
p['time'] = datetime.fromtimestamp(
tp.seconds + tp.centiseconds / 1e2, tz=timezone.utc)
p['latitude'] = tp.latitudeE7 / 1e7
p['longitude'] = tp.longitudeE7 / 1e7
p['sog'] = tp.sog_knotsE1 / 1e1
p['cog'] = tp.cog
p['q1'] = tp.q1E3 / 1e3
p['q2'] = tp.q2E3 / 1e3
p['q3'] = tp.q3E3 / 1e3
p['q4'] = tp.q4E3 / 1e3
quaternion = [p['q1'], p['q2'], p['q3'], p['q4']]
angles = transformations.euler_from_quaternion(quaternion)
p['mag_heading'] = -np.degrees(angles[2]) % 360
# Heel to starboard is positive.
p['heel'] = np.degrees(angles[0])
# Bow up is positive.
p['pitch'] = -np.degrees(angles[1])
points.append(p)
else:
print("Not a position report record: {}".format(r))
return points
def calc_vel(self,pos_x,pos_y,quater):
global odom_key
if odom_key==True:
self.first_x=pos_x
self.first_y=pos_y
self.x_goal+=self.first_x
self.y_goal+=self.first_y
odom_key=False
rel_x=self.x_goal-pos_x
rel_y=self.y_goal-pos_y
dest_angle=math.atan2(rel_y,rel_x)
quaterArr=np.array([quater.x,quater.y,quater.z,quater.w])
robot_euler=transformations.euler_from_quaternion(quaterArr,'sxyz')
robot_angle=robot_euler[2]
angle=dest_angle-robot_angle
if math.sqrt(rel_x**2+rel_y**2) < 0.1:
self.v=0.0;
else :
self.v=self.odom_init_v
if angle > 0.3 :
self.w=self.odom_init_w
elif angle < -0.3:
self.w=-self.odom_init_w
else :
self.w=0.0
return (self.v,self.w)
def quaternion_to_tangent(q, ref_vector=REF_VECTOR):
e = euler_from_quaternion(q)
angle = e[1]
sa = math.sin(angle)
ca = math.cos(angle)
m = np.array([[ca, -sa], [sa, ca]])
return np.dot(m, ref_vector)
def est_err_callback(self, gt_msg, est_msg):
# Change incoming ground truth quarternion data into euler [rad]
gt_quaternion = (gt_msg.pose.pose.orientation.x,
gt_msg.pose.pose.orientation.y,
gt_msg.pose.pose.orientation.z,
gt_msg.pose.pose.orientation.w)
gt_euler = np.unwrap(euler_from_quaternion(gt_quaternion))
# Instantiate arrays and variables
self.gt_pose = array([[gt_msg.pose.pose.position.x],
[gt_msg.pose.pose.position.y],
[gt_msg.pose.pose.position.z],
[np.rad2deg(gt_euler[0])],
[np.rad2deg(gt_euler[1])],
[np.rad2deg(gt_euler[2])],
[gt_msg.twist.twist.linear.x],
[gt_msg.twist.twist.linear.y],
[gt_msg.twist.twist.linear.z]
]) # ground truth array
self.est_pose = array([[est_msg.state.x], [est_msg.state.y],
[est_msg.state.z], [est_msg.state.roll],
[est_msg.state.pitch], [est_msg.state.yaw],
[est_msg.state.vx], [est_msg.state.vy],
[est_msg.state.vz]]) # estimator array
self.est_dist_traveled, self.est_dist_error_rmse = Err.EstAnalysis(
self.gt_pose, self.est_pose)
# Publish
self.PublishEst()
def create_m_constraint(side, transform, set_rotation=False):
""" Create MConstraint to be applied after the MMU is finished
"""
if side == LEFT:
joint_type = MEndeffectorType.LeftHand
else:
joint_type = MEndeffectorType.RightHand
name = "mg_reach_constraint_" + side + str(
datetime.now().strftime("%d%m%y_%H%M%S"))
p = [transform.Position.X, transform.Position.Y, transform.Position.Z]
t = MTranslationConstraint(
MTranslationConstraintType.BOX,
MInterval3(MInterval(p[0], p[0]), MInterval(p[1], p[1]),
MInterval(p[2], p[2])))
if set_rotation:
q = [
transform.Rotation.W, transform.Rotation.X, transform.Rotation.Y,
transform.Rotation.Z
]
e = euler_from_quaternion(q)
r = MRotationConstraint(
MInterval3(MInterval(e[0], e[0]), MInterval(e[1], e[1]),
MInterval(e[2], e[2])))
else:
r = None
geometry_cosntraint = MGeometryConstraint(None,
TranslationConstraint=t,
RotationConstraint=r)
joint_constraint = MJointConstraint(JointType=joint_type,
GeometryConstraint=geometry_cosntraint)
constraint = MConstraint(ID=name, JointConstraint=joint_constraint)
return constraint
def DrCallback(self, msg):
self.dr_update = 1
# Change incoming dead reckoning quarternion data into euler [rad]
dr_quaternion = np.array([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
dr_euler = euler_from_quaternion(dr_quaternion)
unwrapEuler = np.unwrap(dr_euler) # unwrap euler angles
dr_mapEulAng = np.array([[unwrapEuler[0]], [unwrapEuler[1]],
[unwrapEuler[2]]
]) # dr angular position array
dr_pose = array([[msg.pose.pose.position.x
], [msg.pose.pose.position.y],
[msg.pose.pose.position.z],
[np.rad2deg(dr_mapEulAng[0])],
[np.rad2deg(dr_mapEulAng[1])],
[np.rad2deg(dr_mapEulAng[2])],
[msg.twist.twist.linear.x],
[msg.twist.twist.linear.y],
[msg.twist.twist.linear.z]]) # dead reckoning array
# Initialize compute error
Err.DrCallback(dr_pose)
self.ErrAnalysis()
self.dr_update = 0
def _create_actions_from_unity_format(self):
if self._constraints_dict is None:
return
p = self._constraints_dict["startPosition"]
q = self._constraints_dict["startOrientation"]
start_pose = dict()
start_pose["position"] = np.array([p["x"], p["y"], p["z"]])
q = np.array([q["w"], q["x"], q["y"], q["z"]])
q = quaternion_multiply([0, 0, 1, 0], q)
start_pose["orientation"] = np.degrees(euler_from_quaternion(q))
print("start rotation", start_pose["orientation"])
self._controller.start_pose = start_pose
self._action_sequence = []
print("create objects from constraints")
scene = self._parent.app_manager.getDisplayedScene()
a = [None, []]
a[0] = self._constraints_dict["name"]
a[1] = []
for c in self._constraints_dict["frameConstraints"]:
p = c["position"]
p = np.array([p["x"], p["y"], p["z"]])
# q = c["orientation"]
# c["orientation"] = np.array([q["w"], q["x"], q["y"], q["z"]])
annotation = c["keyframe"]
joint_name = c["joint"]
scene_object = PositionConstraintObject(p, 2.5)
scene.addObject(scene_object)
a[1].append(
ConstraintDefinition(scene_object,
joint_name,
annotation=annotation))
self._action_sequence.append(a)
self.update_action_sequence_list()
def ImuCallback(self, msg):
""" Handle IMU message """
if (not self.imu_last_update_time
or msg.header.stamp.to_sec() - self.imu_last_update_time >
self.imu_interval):
self.imu_last_update_time = msg.header.stamp.to_sec()
# Setup map orientation array
euler = euler_from_quaternion([
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]) # Convert IMU data from quarternion to euler
unwrapEuler = np.unwrap(euler) # unwrap euler angles
imu_rot = np.array([
np.rad2deg(unwrapEuler[0]),
np.rad2deg(unwrapEuler[1]),
np.rad2deg(unwrapEuler[2])
]) # imu angular position array
# data for plots
if self.plot_results:
self.results['measurements (IMU)'].append(
np.concatenate(
(np.array([msg.header.stamp.to_sec()]), imu_rot),
axis=0))
def compute_velocity(self):
#print("pos diff: " , np.diff(self.p_gt,axis=0))
#print("t diff: " , np.diff(self.t_gt,axis=0))
pos_diff = np.diff(self.p_gt,axis=0)
q_diff = np.diff(self.q_es_aligned,axis=0)
t_diff = np.diff(self.t_gt,axis=0)
lin_vel = np.zeros(np.shape(pos_diff))
lin_vel_norm = np.zeros(np.shape(pos_diff)[0])
angle_ypr = np.zeros(np.shape(self.p_gt))
angle_vel = np.zeros(np.shape(pos_diff))
angle_vel_norm = np.zeros(np.shape(pos_diff)[0])
for i in range(np.shape(pos_diff)[0]):
lin_vel[i,:] = pos_diff[i,:]/t_diff[i]
lin_vel_norm[i] = np.linalg.norm(lin_vel[i,:])
angle_ypr[i,:] = tf.euler_from_quaternion(self.q_es_aligned[i, :])
angle_ypr_diff = np.diff(angle_ypr,axis=0)
for i in range(np.shape(pos_diff)[0]):
angle_vel[i,:] = angle_ypr_diff[i,:]/t_diff[i]
angle_vel_norm[i] = np.linalg.norm(angle_vel[i,:])
stats_lin_vel = res_writer.compute_statistics(lin_vel_norm)
stats_ang_vel = res_writer.compute_statistics(angle_vel_norm)
self.velocity['lin_vel'] = stats_lin_vel
self.velocity['ang_vel'] = stats_ang_vel
def GtCallback(self, msg):
# Change incoming ground truth quarternion data into euler [rad]
gt_quaternion = np.array([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
gt_euler = euler_from_quaternion(
gt_quaternion) # Convert gt data from quarternion to euler
unwrapEuler = np.unwrap(gt_euler) # unwrap euler angles
gt_mapEulAng = np.array([[unwrapEuler[0]], [unwrapEuler[1]],
[unwrapEuler[2]]
]) # gt angular position array
gt_pose = array([[msg.pose.pose.position.x
], [msg.pose.pose.position.y],
[msg.pose.pose.position.z],
[np.rad2deg(gt_mapEulAng[0])],
[np.rad2deg(gt_mapEulAng[1])],
[np.rad2deg(gt_mapEulAng[2])],
[msg.twist.twist.linear.x],
[msg.twist.twist.linear.y],
[msg.twist.twist.linear.z]]) # ground truth array
# Initialize compute error
Err.GtCallback(gt_pose)
self.ErrAnalysis()
def Hover(HoverPosition):
global R, f
with robotLock:
R.set_speed(speed=[10, 10, 50, 50])
with print_lock:
print "Speeding up to 10% 'hover'"
printDateTime()
print >> f, "Heading to Z coordinate %.1fmm" % HoverPosition[0][2],
f.flush()
while (R.get_cartesian() != HoverPosition):
R.set_cartesian(HoverPosition)
time.sleep(1)
with print_lock:
a = R.get_cartesian()
print a, HoverPosition
R.set_speed(speed=[1, 1, 50, 50])
with print_lock:
print "Slowing Down to 1% 'hover'"
cartesian = R.get_cartesian()
euler = r2d(list(t.euler_from_quaternion(cartesian[1])))
euler = [round(euler[i], 1) for i in range(len(euler))]
cartesian[1] = euler
with print_lock:
printDateTime()
print >> f, "Hovering at: ", cartesian,
f.flush()
time.sleep(0.2)
saveImgEvent.clear()
saveImgEvent.wait(1)
def calc_control_signals(self):
roll, pitch, yaw = trans.euler_from_quaternion(self.attq)
# Compute control errors in position
ex, ey, ez = self.pos_ref - self.pos
vx, vy, vz = self.vel
self.ex_int += ex * self.dt
self.ey_int += ey * self.dt
self.ez_int += ez * self.dt
phi_ref = K_position_proportional * ex - K_position_derivative * vx + K_position_integral * self.ex_int
theta_ref = - (K_position_proportional * ey - K_position_derivative * vy + K_position_integral * self.ey_int)
psi_ref = K_yaw_derivative * self.yawrate_r
thrust_ref = C * (K_height_proportional * ez - K_height_derivative * vz + K_height_integral * self.ez_int + m*g)
# The code below will simply send the thrust that you can set using
# the keyboard and put all other control signals to zero. It also
# shows how, using numpy, you can threshold the signals to be between
# the lower and upper limits defined by the arrays *_limit
self.roll_r = np.clip(theta_ref, *self.roll_limit)
self.pitch_r = np.clip(phi_ref, *self.pitch_limit)
self.yawrate_r = np.clip(psi_ref, *self.yaw_limit)
self.thrust_r = np.clip(thrust_ref, *self.thrust_limit)
if self.debug and (time.time() - 2.0) > self.last_time_print:
self.last_time_print = time.time()
print("ref: ({:.2f}, {:.2f}, {:.2f}, {:.2f})".format(self.pos_ref[0], self.pos_ref[1], self.pos_ref[2], self.yaw_ref))
print("pos: ({:.2f}, {:.2f}, {:.2f}, {:.2f})".format(self.pos[0], self.pos[1], self.pos[2], yaw))
print("vel: ({:.2f}, {:.2f}, {:.2f})".format(self.vel[1], self.vel[1], self.vel[2]))
print("error: ({:.2f}, {:.2f}, {:.2f})".format(ex, ey, ez))
print("integral: ({:.2f}, {:.2f}, {:.2f})".format(self.ex_int, self.ey_int, self.ez_int))
print("control: ({:.2f}, {:.2f}, {:.2f}, {:.2f})".format(self.roll_r, self.pitch_r, self.yawrate_r, self.thrust_r))
print("")
def ImuCallback(self, msg):
# Instantiate imu variables
imu_time = rospy.get_time()
# Setup robot acceleration array
imu_rbtLinAcc = array([[msg.linear_acceleration.x],
[msg.linear_acceleration.y],
[msg.linear_acceleration.z]])
# Setup robot angular velocity array
imu_rbtAngVel = np.array([[msg.angular_velocity.x],
[msg.angular_velocity.y],
[msg.angular_velocity.z]])
# Setup biases array
imu_accBias = array([[self.sen_imu_accBiasX], [self.sen_imu_accBiasY],
[self.sen_imu_accBiasZ]]) # initial accel bias
imu_gyrBias = array([[self.sen_imu_gyrBiasX], [self.sen_imu_gyrBiasY],
[self.sen_imu_gyrBiasZ]]) # initial gyro bias
# Setup map orientation array
euler = euler_from_quaternion([
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]) # Convert IMU data from quarternion to euler
unwrapEuler = np.unwrap(euler) # unwrap euler angles
imu_mapEulAng = np.array([[unwrapEuler[0]], [unwrapEuler[1]],
[unwrapEuler[2]]
]) # imu angular position array
### NOTES: Incoming IMU data is in rotation matrix form ###
imu_mapAngPos = Rot(imu_mapEulAng[0], imu_mapEulAng[1],
imu_mapEulAng[2])
# Initialize estimator
self.uuvEkfEst.ImuCallback(imu_rbtLinAcc, imu_rbtAngVel, imu_mapAngPos,
imu_mapEulAng, imu_accBias, imu_gyrBias,
imu_time)
self.Estimator()
def update(self, tentacle):
# Move actor
self.SetPosition(tentacle.get_position())
self.SetOrientation(0, 0, 0)
axis, angle = quaternion_to_axis_angle(tentacle.get_rotation())
# print(np.round(axis, 3), int(angle))
# self.SetOrientation(tf.euler_from_quaternion(tentacle.get_rotation(), axes='sxyz'))
self.RotateWXYZ(angle, axis[0], axis[1], axis[2])
# Move particles
positions = tentacle.get_local_particle_positions()
rotations = tentacle.get_local_particle_rotations()
for i, pos, rot in zip(range(len(positions)), positions, rotations):
if i < len(self.transforms) and i > 0:
self.transforms[i].Identity()
self.transforms[i].PostMultiply()
self.transforms[i].Translate(
[0, 0, -i * tentacle.radius * self.GetScale()[0] * 2])
x, y, z = tf.euler_from_quaternion(rot, axes='sxyz')
# self.transforms[i].RotateZ(z)
# self.transforms[i].RotateY(y)
# self.transforms[i].RotateX(x)
axis, angle = quaternion_to_axis_angle(rot)
# print(np.round(axis, 3), int(angle))
self.transforms[i].RotateWXYZ(angle, axis)
self.transforms[i].Translate(pos[0] * self.GetScale()[0],
pos[1] * self.GetScale()[1],
pos[2] * self.GetScale()[2])
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 leu_imu(dado):
global linearAcelX
linearAcelX = dado.linear_acceleration.x + 0.75
quat = dado.orientation
lista = [quat.x, quat.y, quat.z, quat.w]
angulos = np.degrees(transformations.euler_from_quaternion(lista))
def getWeight(self):
self.LoadCellArray = [("Time", "ADC", "Weight", "avgWeight", "cartesian[0][0]", "cartesian[0][1]", "cartesian[0][2]", "cartesian[1][0]", "cartesian[1][1]", "cartesian[1][2]", "cartesian[1][3]")]
writeList2File(os.path.join("TSP_Pictures", "%dgrams" % self.kg, "%02d" % self.numFolders, "LoadCellData.txt"), self.LoadCellArray)
while checkData.wait(0) and not end.wait(0):
try:
scaleArray = q1.get(True, 0.1)
q1.task_done()
except Queue.Empty:
break
if robotLock.acquire(False):
cartesian = self.R.get_cartesian()
cartesian1 = copy.copy(cartesian)
robotLock.release()
else:
cartesian = copy.copy(cartesian1)
touchDownQueue.put(cartesian)
cartesian[1] = self.r2d(list(t.euler_from_quaternion(cartesian[1], 'sxyz')))
checktime = time.time()
self.LoadCellArray = ((round((checktime - self.startclock), 6), scaleArray[0], scaleArray[1], scaleArray[2], cartesian1[0][0], cartesian1[0][1], cartesian1[0][2], cartesian1[1][0], cartesian1[1][1], cartesian1[1][2], cartesian1[1][3]))
writeList2File(os.path.join("TSP_Pictures", "%dgramsSH" % self.kg, "%02d" % self.numFolders, "LoadCellData.txt"), self.LoadCellArray)
Z1.put(scaleArray)
Z2.put(scaleArray[2])
Z.set()
time.sleep(0.2)
with print_lock:
print "No Data1"
def set_vel(self, velocity):
"""Publish robot's velocity"""
if self.robot_id != 0:
# External
if self.ext:
t = rospy.get_time()
ext = 0.1*math.sin(t*self.robot_id-self.robot_id)
self.ext_logger.debug(f'{rospy.get_time()},{ext}')
velocity.linear.x += ext
velocity.linear.y += ext
# Send control to dynamic model
velocity.linear.x = self.speed_model_x.update_state(
velocity.linear.x, rospy.get_time())
velocity.linear.y = self.speed_model_y.update_state(
velocity.linear.y, rospy.get_time())
# Orientation correction
(roll, pitch, yaw) = euler_from_quaternion(self.orientation)
velocity.linear.x = velocity.linear.x * math.cos(-roll)
velocity.linear.y = velocity.linear.y * math.sin(-roll + math.pi / 2)
# Publish velocity
self.publisher.publish(velocity)
self.vel_logger.debug(f'{rospy.get_time()},{velocity.linear.x},{velocity.linear.y}')
def goal_callback(self, msg):
goal_x = msg.pose.position.x
goal_y = msg.pose.position.y
q = msg.pose.orientation
goal_th = euler_from_quaternion(np.array([q.x, q.y, q.z, q.w]))[2]
goal_config = np.array([goal_x, goal_y, goal_th])
self.PlanPath(goal_config)
def Home(HomePos):
global R, f
with robotLock:
R.set_speed(speed=[10, 10, 50, 50])
with print_lock:
print "Speeding up to 10%"
printDateTime()
print >> f, "Heading to Home Position",
f.flush()
while (R.get_joints() != HomePos):
R.set_joints(HomePos)
time.sleep(0.5)
with print_lock:
a = R.get_joints()
print a, HomePos
R.set_speed(speed=[1, 1, 50, 50])
with print_lock:
print "Slowing Down to 1%"
cartesian = R.get_cartesian()
euler = r2d(list(t.euler_from_quaternion(cartesian[1])))
euler = [round(euler[i], 1) for i in range(len(euler))]
cartesian[1] = euler
with print_lock:
printDateTime()
print >> f, "Arrived at Home Position: ", cartesian
f.flush()
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 calc_control_signals(self):
# THIS IS WHERE YOU SHOULD PUT YOUR CONTROL CODE
# THAT OUTPUTS THE REFERENCE VALUES FOR
# ROLL PITCH, YAWRATE AND THRUST
# WHICH ARE TAKEN CARE OF BY THE ONBOARD CONTROL LOOPS
roll, pitch, yaw = trans.euler_from_quaternion(self.attq)
# Compute control errors in position
ex, ey, ez = self.pos_ref - self.pos
# The code below will simply send the thrust that you can set using
# the keyboard and put all other control signals to zero. It also
# shows how, using numpy, you can threshold the signals to be between
# the lower and upper limits defined by the arrays *_limit
self.roll_r = np.clip(0.0, *self.roll_limit)
self.pitch_r = np.clip(0.0, *self.pitch_limit)
self.yawrate_r = np.clip(0.0, *self.yaw_limit)
self.thrust_r = np.clip(self.thrust_r, *self.thrust_limit)
message = (
'ref: ({}, {}, {}, {})\n'.format(self.pos_ref[0], self.pos_ref[1],
self.pos_ref[2], self.yaw_ref) +
'pos: ({}, {}, {}, {})\n'.format(self.pos[0], self.pos[1],
self.pos[2], yaw) +
'vel: ({}, {}, {})\n'.format(self.vel[1], self.vel[1], self.vel[2])
+ 'error: ({}, {}, {})\n'.format(ex, ey, ez) +
'control: ({}, {}, {}, {})\n'.format(
self.roll_r, self.pitch_r, self.yawrate_r, self.thrust_r))
self.print_at_period(2.0, message)
def draw_box(self, marker, lifetime, color):
"""
draw box from icv marker
"""
box = carla.BoundingBox()
box.location.x = marker.pose.position.x
box.location.y = -marker.pose.position.y
box.location.z = marker.pose.position.z
box.extent.x = marker.scale.x / 2
box.extent.y = marker.scale.y / 2
box.extent.z = marker.scale.z / 2
roll, pitch, yaw = euler_from_quaternion([
marker.pose.orientation.x,
marker.pose.orientation.y,
marker.pose.orientation.z,
marker.pose.orientation.w
])
rotation = carla.Rotation()
rotation.roll = math.degrees(roll)
rotation.pitch = math.degrees(pitch)
rotation.yaw = -math.degrees(yaw)
#rospy.loginfo("Draw Box {} (rotation: {}, color: {}, lifetime: {})".format(
# box, rotation, color, lifetime))
self.debug.draw_box(box, rotation, thickness=0.1, color=color, life_time=lifetime)
def hover(self):
with robotLock:
self.R.set_speed(speed=[10, 10, 50, 50])
print "Speeding up to 10% 'hover'"
self.printDateTime()
print >> self.f, "Heading to Z coordinate %.1fmm" % self.HoverPosition[0][2],
print "Heading to Z coordinate %.1fmm" % self.HoverPosition[0][2]
self.f.flush()
count = 0
while (self.R.get_cartesian() != self.HoverPosition):
self.R.set_cartesian(self.HoverPosition)
time.sleep(1)
a = self.R.get_cartesian()
print a, self.HoverPosition
if count >= 3:
self.R.set_speed(speed=[0.5, 0.5, 50, 50])
count += 1
self.R.set_speed(speed=[0.5, 0.5, 50, 50])
print "Slowing Down to 1% 'hover'"
cartesian = self.R.get_cartesian()
euler = self.r2d(t.euler_from_quaternion(cartesian[1]))
euler = [round(euler[i], 1) for i in range(len(euler))]
cartesian[1] = euler
self.printDateTime()
print >> self.f, "Hovering at: ", cartesian
self.f.flush()
time.sleep(0.2)
saveImage()
def readM100Data(sourceFileName, uwbTime, uwb, imuTime, yaw, acc, gyro,
velTime, vel, pos):
# cloct data for ekf
expEuler = []
file = open(sourceFileName, 'r')
print(sourceFileName)
numeric_const_pattern = '[-+]? (?: (?: \d* \. \d+ ) | (?: \d+ \.? ) )(?: [Ee] [+-]? \d+ ) ?'
rx = re.compile(numeric_const_pattern, re.VERBOSE)
with open(sourceFileName, 'r') as file:
for line in file:
dis = rx.findall(line)
#print(dis)
if line[0] == "u":
temp = float(dis[0]) + float(
dis[1]) * 10**-9 # time tamp of imu measurement
uwbTime.append(temp)
uwb.append(float(dis[2]) / 1000)
#print("add uwb", uwb[-1])
pass
elif line[0] == "i":
temp = float(dis[0]) + float(
dis[1]) * 10**-9 # time tamp of imu measurement
imuTime.append(temp)
quater = [
float(dis[3]),
float(dis[4]),
float(dis[5]),
float(dis[2])
] # x-y-z-w
expEuler = euler_from_quaternion(quater)
yaw.append(expEuler[2])
temp = []
temp = [float(dis[6]), float(dis[7]), float(dis[8])]
acc.append(temp)
temp = []
temp = [float(dis[9]), float(dis[10]), float(dis[11])]
gyro.append(temp)
pass
elif line[0] == "v":
temp = float(dis[0]) + float(
dis[1]) * 10**-9 # time tamp of imu measurement
velTime.append(temp)
temp = [float(dis[2]), float(dis[3]), float(dis[4])]
vel.append(temp)
pass
elif line[0] == "p":
temp = [float(dis[2]), float(dis[3]), float(dis[4])]
pos.append(temp)
pass
#gtfile = pd.read_csv(optitrackFileName, header=0, skiprows=6, engine='python')
print("data size -range ", len(uwb), " angle: ", len(yaw))
pass
def get_euler_angles(self):
for i in range(len(self.timestamp)):
# [self.euler_x[i], self.euler_y[i], self.euler_z[i]] =
euler = euler_from_quaternion((self.x_values[i], self.y_values[i],
self.z_values[i], self.w_values[i]))
self.euler_x.append(euler[0])
self.euler_y.append(euler[1])
self.euler_z.append(euler[2])
def get_node_args(self):
r, p, y = transformations.euler_from_quaternion([
self.pose.orientation.x, self.pose.orientation.y,
self.pose.orientation.z, self.pose.orientation.w
])
return "{0.x} {0.y} {0.z} " \
"{1} {2} {3} {4} {5} 100" \
.format(self.pose.position, y, p, r, self.parent_frame, self.child_frame)
def imu_callback(imu):
global imu_euler, imu_quaternion
imu_quaternion = (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w)
roll, pitch, yaw = euler_from_quaternion(imu_quaternion, axes="sxyz")
imu_euler = RPY()
imu_euler.roll = roll
imu_euler.pitch = pitch
imu_euler.yaw = yaw
def pose_callback(msg):
xyz = msg.pose.position
quat = msg.pose.orientation
x, y, z = xyz.x, xyz.y, xyz.z
roll, pitch, yaw = tf.euler_from_quaternion(
[quat.w, quat.x, quat.y, quat.z], axes='sxyz')
path.append([x, y, z, roll, pitch, yaw])
timestamps.append(msg.header.stamp.secs + 1e-9 * msg.header.stamp.nsecs)
def new_marker(M, id, x, y, z, q1, q2, q3, q4):
q4_ = math.sqrt(1.0 - q1 * q1 - q2 * q2 - q3 * q3)
e = transformations.euler_from_quaternion([q1, q2, q3, q4_])
if str(id) not in M:
M[str(id)] = [[x, y, z, e[0], e[1], e[2]]]
else:
# print M[str(id)]
M[str(id)].append([x, y, z, e[0], e[1], e[2]])
def lasers_callback(data):
global report
quaternion = (data.pose.orientation.x, data.pose.orientation.y, \
data.pose.orientation.z, data.pose.orientation.w)
roll, pitch, yaw = euler_from_quaternion(quaternion, axes="sxyz")
report.lasers_pose.position = data.pose.position
report.lasers_pose.orientation.roll = roll
report.lasers_pose.orientation.pitch = pitch
report.lasers_pose.orientation.yaw = yaw
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 saveModelStates (self, model_states):
# Model states comes in as a list of all entities in the enviroment
m = model_states.name.index ("mobile_base")
self.turtlebot_position = model_states.pose[m].position
self.turtlebot_orientation = model_states.pose[m].orientation
# Getting yaw (in x, y, z) rather than a quaternion
self.euler_angles = tf.euler_from_quaternion([self.turtlebot_orientation.w, self.turtlebot_orientation.x, self.turtlebot_orientation.y, self.turtlebot_orientation.z])
# Yaw is in range [0, 360)
self.yaw = degrees ((pi - (self.euler_angles[0])))
self.compass = (self.yaw - degrees (pi/4)) // degrees ((pi / 2))
if self.compass == -1.0:
self.compass = 3.0
def callback2(odom,rob):
global MSG
#MSG[rob][:]=numpy.zeros(18)
MSG[rob][0]=odom.pose.pose.position.x
MSG[rob][1]=odom.pose.pose.position.y
MSG[rob][2]=odom.pose.pose.position.z
rot = np.empty((4, ), dtype=np.float64)
rot[0]=odom.pose.pose.orientation.x
rot[1]=odom.pose.pose.orientation.y
rot[2]=odom.pose.pose.orientation.z
rot[3]=odom.pose.pose.orientation.w
(roll,pitch,yaw) = euler_from_quaternion(rot)
MSG[rob][3]= round(yaw,5)
MSG[rob][4]= odom.twist.twist.linear.x
MSG[rob][5]= round(odom.twist.twist.angular.z,4)
MSG[rob][6]= odom.header.stamp.secs
MSG[rob][7]= odom.header.stamp.nsecs
def getPos(self):
rate=rospy.Rate(10.0)
while not rospy.is_shutdown():
print "here1"
try:
#Pulled from here: http://answers.ros.org/question/10268/where-am-i-in-the-map/
# (trans,rot) = self.listener.lookupTransform('map', 'odom', rospy.Time(0))
(trans,rot) = self.listener.lookupTransform('map', 'base_link', rospy.Time(0))
print "after lookup"
rot=transformations.euler_from_quaternion(rot)
return (trans,rot)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException ) as e:
print e
continue
rate.sleep()
def Home(self):
with robotLock:
self.R.set_speed(speed=[10, 10, 50, 50])
print "Speeding up to 10%"
self.printDateTime()
print >> self.f, "Heading to Home Position",
self.f.flush()
while (self.R.get_joints() != self.homePos):
self.R.set_joints(self.homePos)
time.sleep(4)
self.R.set_speed(speed=[0.5, 0.5, 50, 50])
print "Slowing Down to 1%"
cartesian = self.R.get_cartesian()
euler = self.r2d(t.euler_from_quaternion(cartesian[1]))
euler = [round(euler[i], 1) for i in range(len(euler))]
cartesian[1] = euler
self.printDateTime()
print >> self.f, "Arrived at Home Position: ", cartesian
self.f.flush()
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 record(self):
self.Array = [("Time", "cartesian[0][0]", "cartesian[0][1]", "cartesian[0][2]", "cartesian[1][0]", "cartesian[1][1]", "cartesian[1][2]", "cartesian[1][3]")]
writeList2File(os.path.join(self.DIR, "RunData.txt"), self.Array)
while not end.wait(0):
if robotLock.acquire(False):
cartesian = self.R.get_cartesian()
cartesian1 = copy.deepcopy(cartesian)
robotLock.release()
else:
cartesian = copy.deepcopy(cartesian1)
touchDownQueue.put(cartesian)
cartesian[1] = self.r2d(t.euler_from_quaternion(cartesian[1], 'sxyz'))
checktime = time.time()
self.Array = ((round((checktime - self.startclock), 6), cartesian1[0][0], cartesian1[0][1], cartesian1[0][2], cartesian1[1][0], cartesian1[1][1], cartesian1[1][2], cartesian1[1][3]))
writeList2File(os.path.join(self.DIR, "RunData.txt"), self.Array)
ok.set()
time.sleep(0.2)
with print_lock:
print "No Data1"
def quat2ypr(quaternions,method='kevin'):
"""
Purpose: Convert a list of arrays of quaternions to Euler angles (roll,pitch,yaw)
Inputs: quaternions - list of quaternion arrays. In the form of 'SXYZ' I THINK?!?!?
Outputs: r,p,y - separate roll, pitch, and yaw vectors
Can test with:
>>>quats=tfdat /= abs(fdat).max()racking.sample_quats()
>>>[r,p,y]=tracking.quat2rpy(quats)
"""
roll=[]
pitch=[]
yaw=[]
for q in quaternions:
if method is 'transform':
YPR=transform.euler_from_quaternion(q, axes='rzyx') # default is 'sxyz'
else:
YPR= quat2euler321(q)
yaw.append(YPR[0])
pitch.append(YPR[1])
roll.append(YPR[2])
return yaw,pitch,roll
def hover(self):
with robotLock:
initErrorBds.set()
self.error = 5
self.R.set_speed(speed=[10, 10, 50, 50])
with print_lock:
print "Speeding up to 10% 'hover'"
self.printDateTime()
print >> self.f, "Heading to Z coordinate 345mm",
self.f.flush()
while (self.R.get_cartesian() != self.HoverPosition):
self.R.set_cartesian(self.HoverPosition)
time.sleep(1)
self.R.set_speed(speed=[0.5, 0.5, 50, 50])
with print_lock:
print "Slowing Down to 1% 'hover'"
cartesian = self.R.get_cartesian()
euler = self.r2d(list(t.euler_from_quaternion(cartesian[1])))
euler = [round(euler[i], 1) for i in range(len(euler))]
cartesian[1] = euler
self.printDateTime()
print >> self.f, "Hovering at: ", cartesian
self.f.flush()
return cartesian
def solvePnP2( pts_dict ):
# build a new cam_dict that is a copy of the current one
cam_dict = {}
for i1 in proj.image_list:
cam_dict[i1.name] = {}
rvec, tvec = i1.get_proj()
ned, ypr, quat = i1.get_camera_pose()
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = ned
camw, camh = proj.cam.get_image_params()
for i, i1 in enumerate(proj.image_list):
print i1.name
scale = float(i1.width) / float(camw)
K = proj.cam.get_K(scale)
rvec1, tvec1 = i1.get_proj()
cam2body = i1.get_cam2body()
body2cam = i1.get_body2cam()
# include our own position in the average
quat_start_weight = 100
ned_start_weight = 2
count = 0
sum_quat = np.array(i1.camera_pose['quat']) * quat_start_weight
sum_ned = np.array(i1.camera_pose['ned']) * ned_start_weight
for j, i2 in enumerate(proj.image_list):
matches = i1.match_list[j]
if len(matches) < 8:
continue
count += 1
rvec2, tvec2 = i2.get_proj()
# build obj_pts and img_pts to position i1 relative to the
# matches in i2.
img1_pts = []
img2_pts = []
obj_pts = []
for pair in matches:
kp1 = i1.kp_list[ pair[0] ]
img1_pts.append( kp1.pt )
kp2 = i2.kp_list[ pair[1] ]
img2_pts.append( kp2.pt )
key = "%d-%d" % (i, pair[0])
if not key in pts_dict:
key = "%d-%d" % (j, pair[1])
# print key, pts_dict[key]
obj_pts.append(pts_dict[key])
# compute the centroid of obj_pts
sum = np.zeros(3)
for p in obj_pts:
sum += p
obj_center = sum / len(obj_pts)
print "obj_pts center =", obj_center
# given the previously computed triangulations (and
# averages of point 3d locations if they are involved in
# multiple triangulations), then compute an estimate for
# both matching camera poses. The relative positioning of
# these camera poses should be pretty accurate.
(result, rvec1, tvec1) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img1_pts),
K, None, rvec1, tvec1,
useExtrinsicGuess=True)
(result, rvec2, tvec2) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img2_pts),
K, None, rvec2, tvec2,
useExtrinsicGuess=True)
Rned2cam1, jac = cv2.Rodrigues(rvec1)
Rned2cam2, jac = cv2.Rodrigues(rvec2)
ned1 = -np.matrix(Rned2cam1[:3,:3]).T * np.matrix(tvec1)
ned2 = -np.matrix(Rned2cam2[:3,:3]).T * np.matrix(tvec2)
print "cam1 orig=%s new=%s" % (i1.camera_pose['ned'], ned1)
print "cam2 orig=%s new=%s" % (i2.camera_pose['ned'], ned2)
# compute a rigid transform (rotation + translation) to
# align the estimated camera locations (projected from the
# triangulation) back with the original camera points, and
# roughly rotated around the centroid of the object
# points.
src = np.zeros( (3,3) ) # current camera locations
dst = np.zeros( (3,3) ) # original camera locations
src[0,:] = np.squeeze(ned1)
src[1,:] = np.squeeze(ned2)
src[2,:] = obj_center
dst[0,:] = i1.camera_pose['ned']
dst[1,:] = i2.camera_pose['ned']
dst[2,:] = obj_center
print "src:\n", src
print "dst:\n", dst
M = transformations.superimposition_matrix(src, dst)
print "M:\n", M
# Our (i1) Rned2cam matrix is body2cam * Rned, so solvePnP
# returns this combination. We can extract Rbody2ned by
# premultiplying by cam2body aka inv(body2cam).
Rned2body = cam2body.dot(Rned2cam1)
# print "Rned2body:\n", Rned2body
Rbody2ned = np.matrix(Rned2body).T # IR
# print "Rbody2ned:\n", Rbody2ned
# print "R (after M * R):\n", R
# now transform by the earlier "M" affine transform to
# rotate the work space closer to the actual camera points
Rot = M[:3,:3].dot(Rbody2ned)
# make 3x3 rotation matrix into 4x4 homogeneous matrix
hRot = np.concatenate( (np.concatenate( (Rot, np.zeros((3,1))),1),
np.mat([0,0,0,1])) )
# print "hRbody2ned:\n", hRbody2ned
quat = transformations.quaternion_from_matrix(hRot)
sum_quat += quat
sum_ned += np.squeeze(np.asarray(ned1))
print "count = ", count
newned = sum_ned / (ned_start_weight + count)
print "orig ned =", i1.camera_pose['ned']
print "new ned =", newned
newquat = sum_quat / (quat_start_weight + count)
newIR = transformations.quaternion_matrix(newquat)
print "orig ypr = ", i1.camera_pose['ypr']
(yaw, pitch, roll) = transformations.euler_from_quaternion(newquat, 'rzyx')
print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
newR = np.transpose(newIR[:3,:3]) # equivalent to inverting
newRned2cam = body2cam.dot(newR[:3,:3])
rvec, jac = cv2.Rodrigues(newRned2cam)
tvec = -np.matrix(newRned2cam) * np.matrix(newned).T
#print "orig pose:\n", cam_dict[i1.name]
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = newned
#print "new pose:\n", cam_dict[i1.name]
return cam_dict
alphaw = []
alphasail = []
goalr = []
for row in data:
for i in range(len(row)):
if abs(row[i]) > 1e5:
row[i] = float("nan")
t.append(row[0])
sail.append(row[1])
rudder.append(row[2])
x.append(row[3])
y.append(row[4])
vx.append(row[5])
vy.append(row[6])
q = row[7:11]
e = transformations.euler_from_quaternion(q, axes='rzxy')
yaw.append(e[0])
heel.append(e[1])
pitch.append(e[2])
rotmat = transformations.quaternion_matrix(q)
yaw_true.append(numpy.arctan2(rotmat[1, 0], rotmat[0, 0]))
windx = row[11]
windy = row[12]
alphaw.append(numpy.arctan2(vy[-1] - windy, vx[-1] - windx) - yaw_true[-1])
alphasail.append(alphaw[-1] - sail[-1])
goalr.append(numpy.clip(yaw_true[-1] - .1, -.4, .4))
plt.plot(x, y, 'o')
plt.figure()
ax = plt.subplot(111)
ax.plot(t, x, label='x')
def simulateWeight(self, Position):
oldPos = Position
cG = 0
while not end.wait(0):
Z.wait()
try:
avgWeight = Z2.get(True, 0.1)
except Queue.Empty:
break
if avgWeight < self.kg - self.error:
Position[0][2] = round(Position[0][2] - 0.1, 2)
with print_lock:
print "DOWN"
with robotLock:
while (self.R.get_cartesian() != Position) and not end.wait(0):
self.R.set_cartesian(Position)
time.sleep(1)
with print_lock:
a = self.R.get_cartesian()
print a, Position
oldPos = Position
isTouchedDown.clear()
if Position[0][2] == 380:
with print_lock:
print "LOAD CELL ERROR - Min"
self.Home()
self.close()
cG = 0
elif avgWeight > self.kg + self.error:
Position[0][2] = round(Position[0][2] + 0.1, 2)
with print_lock:
print "UP"
with robotLock:
while (self.R.get_cartesian() != Position) and not end.wait(0):
self.R.set_cartesian(Position)
time.sleep(1)
with print_lock:
a = self.R.get_cartesian()
print a, Position
if [abs(b) for b in a[1]] in [[0.172, 0.021, 0.978, 0.12], [0.311, 0.0, 0.95, 0.0]]:
break
oldPos = Position
isTouchedDown.clear()
if Position[0][2] == 460:
with print_lock:
print "LOAD CELL ERROR - Max"
self.Home()
self.close()
cG = 0
else:
with print_lock:
print "Touched Down"
with robotLock:
euler = copy.copy(Position)
euler[1] = self.r2d(list(t.euler_from_quaternion(euler[1])))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Target: ", euler,
while (self.R.get_cartesian() != Position) and not end.wait(0):
self.R.set_cartesian(Position)
time.sleep(1)
with print_lock:
a = self.R.get_cartesian()
print a, Position
if [abs(b) for b in a[1]] in [[0, 0, 1, 0], [0.174, 0.004, 0.984, 0.024], [0.172, 0.021, 0.978, 0.12], [0.311, 0, 0.95, 0], [0.326, 0, 0.945, 0], [0.151, 0, 0.989, 0], [0.105, 0, 0.994, 0], [0.174, 0.024, 0.984, 0.004], [0.172, 0.12, 0.978, 0.021]]:#, [0.172, 0.12, 0.978, 0.021], [0.271, 0, 0.963, 0], [0.326, 0, 0.945, 0], [0.105, 0, 0.994, 0]]:
break
a = self.R.get_cartesian()
oldPos = Position
euler = copy.copy(a)
euler[1] = self.r2d(list(t.euler_from_quaternion(euler[1])))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Result: ", euler,
self.f.flush()
with print_lock:
print euler
cG += 1
if cG == 2:
cG = 0
if initErrorBds.wait(0):
initErrorBds.clear()
self.error = 7.5
break
isTouchedDown.set()
Z.clear()
time.sleep(2)
return oldPos
import transformations
# start with the identity
#sum = transformations.quaternion_from_euler(0.0, 0.0, 0.0, axes='sxyz')
sum = np.zeros(4)
count = 0
for i in range(0,1000):
rot = random.random()*0.25-0.125
print "rotation =", rot
quat = transformations.quaternion_about_axis(rot, [1, 0, 0])
print "quat =", quat
count += 1
sum[0] += quat[0]
sum[1] += quat[1]
sum[2] += quat[2]
sum[3] += quat[3]
w = sum[0] / float(count)
x = sum[1] / float(count)
y = sum[2] / float(count)
z = sum[3] / float(count)
new_quat = np.array( [ w, x, y, z] )
print "new_quat (raw) =", new_quat
# normalize ...
new_quat = new_quat / math.sqrt(np.dot(new_quat, new_quat))
print " avg =", new_quat
print " eulers =", transformations.euler_from_quaternion(new_quat, 'sxyz')
def runMain(self, units, i):
if is_runningEvent.wait(0):
with Newtons_lock:
d = dic.get()
dic.task_done()
dic.put(d)
data = d[self.movType[i]]
DIR = os.path.join(self.DIR, self.movType[i])
self.makedir(DIR)
self.directory = Queue.LifoQueue()
self.directory.put(DIR)
self.printDateTime()
print >> self.f, "Starting Sequence: %s 0%s --> -20%s" % (DIR, units, units)
self.hover()
time.sleep(0.5)
for dataline in data[:len(data) / 2]:
if is_runningEvent.wait(0):
dataline = [dataline[:3], dataline[3:]]
euler = copy.deepcopy(dataline)
euler[1] = self.r2d(t.euler_from_quaternion(euler[1]))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Target: ", euler,
print "Starting Movement"
with robotLock:
while ([self.R.get_cartesian()[0], self.r2d(t.euler_from_quaternion(self.R.get_cartesian()[1]))] != euler) and is_runningEvent.wait(0): # [dataline[0], [round(line,3) for line in dataline[1]]]
self.R.set_cartesian(dataline)
time.sleep(0.5)
a = self.R.get_cartesian()
b = [self.R.get_cartesian()[0], self.r2d(t.euler_from_quaternion(self.R.get_cartesian()[1]))]
print a, dataline # [dataline[0], [round(line,3) for line in dataline[1]]]
print b, euler
print "Equal? ", [[abs(c1) for c1 in b[0]], [abs(c2) for c2 in b[1]]] == [[abs(e1) for e1 in euler[0]], [abs(e2) for e2 in euler[1]]]
if [[abs(c1) for c1 in b[0]], [abs(c2) for c2 in b[1]]] == [[abs(e1) for e1 in euler[0]], [abs(e2) for e2 in euler[1]]]:
break
euler = self.R.get_cartesian()
euler[1] = self.r2d(t.euler_from_quaternion(euler[1]))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Result: ", euler,
self.f.flush()
print "Result: ", euler
print "Finished Movement"
self.get300samples(self.WeightSamples, euler[0][2])
time.sleep(0.2)
saveImage()
time.sleep(1)
for dataline in data[len(data) / 2 : len(data)]:
if is_runningEvent.wait(0):
dataline = [dataline[:3], dataline[3:]]
euler = copy.deepcopy(dataline)
euler[1] = self.r2d(t.euler_from_quaternion(euler[1]))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Target: ", euler,
print "Starting Movement"
with robotLock:
while ([self.R.get_cartesian()[0], self.r2d(t.euler_from_quaternion(self.R.get_cartesian()[1]))] != euler) and is_runningEvent.wait(0): # [dataline[0], [round(line,3) for line in dataline[1]]]
self.R.set_cartesian(dataline)
time.sleep(0.5)
a = self.R.get_cartesian()
b = [self.R.get_cartesian()[0], self.r2d(t.euler_from_quaternion(self.R.get_cartesian()[1]))]
print a, dataline # [dataline[0], [round(line,3) for line in dataline[1]]]
print b, euler
print "Equal? ", [[abs(c1) for c1 in b[0]], [abs(c2) for c2 in b[1]]] == [[abs(e1) for e1 in euler[0]], [abs(e2) for e2 in euler[1]]]
if [[abs(c1) for c1 in b[0]], [abs(c2) for c2 in b[1]]] == [[abs(e1) for e1 in euler[0]], [abs(e2) for e2 in euler[1]]]:
break
euler = self.R.get_cartesian()
euler[1] = self.r2d(t.euler_from_quaternion(euler[1]))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Result: ", euler,
self.f.flush()
print "Result: ", euler
print "Finished Movement"
self.get300samples(self.WeightSamples, euler[0][2])
time.sleep(0.2)
saveImage()
def solvePnP1( pts_dict ):
# start with a clean slate
for i1 in proj.image_list:
i1.img_pts = []
i1.obj_pts = []
# build a new cam_dict that is a copy of the current one
cam_dict = {}
for i1 in proj.image_list:
cam_dict[i1.name] = {}
rvec, tvec = i1.get_proj()
ned, ypr, quat = i1.get_camera_pose()
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = ned
camw, camh = proj.cam.get_image_params()
for i, i1 in enumerate(proj.image_list):
print i1.name
scale = float(i1.width) / float(camw)
K = proj.cam.get_K(scale)
rvec, tvec = i1.get_proj()
cam2body = i1.get_cam2body()
body2cam = i1.get_body2cam()
# include our own position in the average
count = 1
sum_quat = np.array(i1.camera_pose['quat'])
sum_ned = np.array(i1.camera_pose['ned'])
for j, i2 in enumerate(proj.image_list):
matches = i1.match_list[j]
if len(matches) < 8:
continue
count += 1
# build obj_pts and img_pts to position i1 relative to the
# matches in i2
obj_pts = []
img_pts = []
for pair in matches:
kp = i1.kp_list[ pair[0] ]
img_pts.append( kp.pt )
key = "%d-%d" % (i, pair[0])
if not key in pts_dict:
key = "%d-%d" % (j, pair[1])
# print key, pts_dict[key]
obj_pts.append(pts_dict[key])
#if key in pts_dict:
# sum_dict[key] += p
#else:
# sum_dict[key] = p
(result, rvec, tvec) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img_pts),
K, None, rvec, tvec,
useExtrinsicGuess=True)
#print "rvec=%s tvec=%s" % (rvec, tvec)
Rned2cam, jac = cv2.Rodrigues(rvec)
#print "Rned2cam (from SolvePNP):\n", Rned2cam
# Our Rcam matrix (in our ned coordinate system) is
# body2cam * Rned, so solvePnP returns this combination.
# We can extract Rned by premultiplying by cam2body aka
# inv(body2cam).
Rned2body = cam2body.dot(Rned2cam)
# print "Rned2body:\n", Rned2body
Rbody2ned = np.matrix(Rned2body).T # IR
# print "Rbody2ned:\n", Rbody2ned
# print "R (after M * R):\n", R
# make 3x3 rotation matrix into 4x4 homogeneous matrix
hRbody2ned = np.concatenate( (np.concatenate( (Rbody2ned, np.zeros((3,1))),1), np.mat([0,0,0,1])) )
# print "hRbody2ned:\n", hRbody2ned
quat = transformations.quaternion_from_matrix(hRbody2ned)
sum_quat += quat
pos = -np.matrix(Rned2cam[:3,:3]).T * np.matrix(tvec)
sum_ned += np.squeeze(np.asarray(pos))
print "ned =", np.squeeze(np.asarray(pos))
print "count = ", count
newned = sum_ned / float(count)
print "orig ned =", i1.camera_pose['ned']
print "new ned =", newned
newquat = sum_quat / float(count)
newIR = transformations.quaternion_matrix(newquat)
print "orig ypr = ", i1.camera_pose['ypr']
(yaw, pitch, roll) = transformations.euler_from_quaternion(newquat, 'rzyx')
print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
newR = np.transpose(newIR[:3,:3]) # equivalent to inverting
newRned2cam = body2cam.dot(newR[:3,:3])
rvec, jac = cv2.Rodrigues(newRned2cam)
tvec = -np.matrix(newRned2cam) * np.matrix(newned).T
#print "orig pose:\n", cam_dict[i1.name]
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = newned
#print "new pose:\n", cam_dict[i1.name]
return cam_dict
def animate(i):
lock.acquire()
local_pos = pose[0:2];
local_orient = pose[2]
local_rpy = [pose[3],pose[4],pose[5]]
lock.release()
lock2.acquire()
global global_humans
local_humans = global_humans
lock2.release()
human_wedge1.set_visible(False)
human_wedge2.set_visible(False)
human_wedge3.set_visible(False)
human_wedge4.set_visible(False)
human_wedge5.set_visible(False)
human_wedge6.set_visible(False)
for human in local_humans.human:
id = human.id
tracked = human.tracked
if tracked == True:
temp_wedge = None
if(id == 1):
temp_wedge = human_wedge1
elif(id == 2):
temp_wedge = human_wedge2
elif(id == 3):
temp_wedge = human_wedge3
elif(id == 4):
temp_wedge = human_wedge4
elif(id == 5):
temp_wedge = human_wedge5
elif(id == 6):
temp_wedge = human_wedge6
if temp_wedge != None:
temp_wedge.set_visible(True)
human_torso = human.torso_position
#print human_torso
w = human.orientation.w
x = human.orientation.x
y = human.orientation.y
z = human.orientation.z
human_orient = [human.orientation.w,human.orientation.x,human.orientation.y,human.orientation.z]
#print human_orient
temp_wedge.set_center((human_torso.z,human_torso.x))
human_euler = transformations.euler_from_quaternion(human_orient,axes='szyx')
if(not(math.isnan(w)) and not(math.isnan(x)) and not(math.isnan(y)) and not(math.isnan(z))):
roll = math.degrees(math.atan2(2*y*w - 2*x*z, 1 - 2*y*y - 2*z*z));
pitch = math.degrees(math.atan2(2*x*w - 2*y*z, 1 - 2*x*x - 2*z*z));
yaw = math.degrees(math.asin(2*x*y + 2*z*w));
human_euler = [math.degrees(x) for x in human_euler]
human_deg = 180-roll
temp_wedge.set_theta1(human_deg - 15)
temp_wedge.set_theta2(human_deg + 15)
print "Human : " , human.id, " , " , [human_torso.z,human_torso.x], " , " , [roll,pitch,yaw]
#local_pos = [x/1000 for x in local_pos]
#euler = transformations.euler_from_quaternion(local_orient,axes='syxz')
#euler = [math.degrees(x) for x in euler]
#deg = euler[1]
deg = math.degrees(local_orient)
if(deg > 0):
deg = deg-90
else:
deg = deg+90
#x,y = wedge1.center
x = local_pos[0]
y = local_pos[1]
wedge1.set_center((x,y))
wedge1.set_theta1(deg - 15)
wedge1.set_theta2(deg + 15)
#print "Hello : (%d,%d);(%d,%d)" % (x,y,theta1,theta2)
local_rpy = [math.degrees(x) for x in local_rpy]
print local_pos,deg,local_rpy
#print "Position : " , local_pos
#print "Orientation : " , local_orient
#print "Euler : " , deg
return [wedge1,human_wedge1,human_wedge2,human_wedge3,human_wedge4,human_wedge5,human_wedge6],
def as6Dpose(self, pose):
wx, wy, wz = transformations.euler_from_quaternion([pose['qx'], pose['qy'], pose['qz'], pose['qw']], axes= 'sxyz')
return [pose['x'], pose['y'], pose['z'], wx, wy, wz]
def _draw_3dim_angle(self, quaternion):
x, y, z = euler_from_quaternion(quaternion, "rxyz")
glRotatef(math.degrees(x), 1., 0., 0.)
glRotatef(math.degrees(y), 0., 1., 0.)
glRotatef(math.degrees(z), 0., 0., 1.)
self.draw_cuboid_shape()
def runMain(self, units, i):
if checkData.wait(0) or not end.wait(0):
with Newtons_lock:
d = dic.get()
dic.task_done()
dic.put(d)
data = d[self.movType[i]]
DIR = os.path.join(self.DIR, self.movType[i], "P")
self.makedir(DIR)
self.directory = Queue.LifoQueue()
self.directory.put(DIR)
# print >> self.f, "\n############################################################################################################"
self.printDateTime()
print >> self.f, "Starting Sequence: %s 0%s --> +20%s" % (DIR, units, units)
self.hover()
time.sleep(1)
for dataline in data[:len(data) / 2]:
dataline = [dataline[:3], dataline[3:]]
with robotLock:
euler = copy.copy(dataline)
euler[1] = self.r2d(list(t.euler_from_quaternion(euler[1])))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Target: ", euler,
print "Starting Movement"
while (self.R.get_cartesian() != dataline) and not end.wait(0):
self.R.set_cartesian(dataline)
time.sleep(0.5)
with print_lock:
a = self.R.get_cartesian()
print a, dataline
if [abs(b) for b in a[1]] in [[0, 0, 1, 0], [0.174, 0.004, 0.984, 0.024], [0.172, 0.021, 0.978, 0.12], [0.311, 0, 0.95, 0], [0.326, 0, 0.945, 0], [0.151, 0, 0.989, 0], [0.105, 0, 0.994, 0], [0.174, 0.024, 0.984, 0.004], [0.172, 0.12, 0.978, 0.021]]:#, [0.172, 0.12, 0.978, 0.021], [0.271, 0, 0.963, 0], [0.326, 0, 0.945, 0], [0.105, 0, 0.994, 0]]:
break
a = self.R.get_cartesian()
euler = copy.copy(a)
euler[1] = self.r2d(list(t.euler_from_quaternion(euler[1])))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Result: ", euler,
self.f.flush()
with print_lock:
print euler
print "Finished Movement"
time.sleep(0.2)
saveImgEvent.clear()
if not saveImgEvent.wait(1):
break
with Newtons_lock:
DIR = os.path.join(DIR[:-2], "N")
self.makedir(DIR)
self.directory = Queue.LifoQueue()
self.directory.put(DIR)
# print >> self.f, "\n############################################################################################################"
self.printDateTime()
print >> self.f, "Starting Sequence: %s 0%s --> -20%s" % (DIR, units, units)
oldPos = self.hover()
time.sleep(1)
for dataline in data[len(data) / 2:len(data)]:
dataline = [dataline[:3], dataline[3:]]
with robotLock:
euler = copy.copy(dataline)
euler[1] = self.r2d(list(t.euler_from_quaternion(euler[1])))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Target: ", euler,
print "Starting Movement"
while (self.R.get_cartesian() != dataline) and not end.wait(0):
self.R.set_cartesian(dataline)
time.sleep(1)
with print_lock:
a = self.R.get_cartesian()
print a, dataline
if [abs(b) for b in a[1]] in [[0, 0, 1, 0], [0.174, 0.004, 0.984, 0.024], [0.172, 0.021, 0.978, 0.12], [0.311, 0, 0.95, 0], [0.326, 0, 0.945, 0], [0.151, 0, 0.989, 0], [0.105, 0, 0.994, 0], [0.174, 0.024, 0.984, 0.004], [0.172, 0.12, 0.978, 0.021]]:#, [0.172, 0.12, 0.978, 0.021], [0.271, 0, 0.963, 0], [0.326, 0, 0.945, 0], [0.105, 0, 0.994, 0]]:
break
a = self.R.get_cartesian()
euler = copy.copy(a)
euler[1] = self.r2d(list(t.euler_from_quaternion(euler[1])))
euler[1] = [round(euler[1][i], 1) for i in range(len(euler[1]))]
self.printDateTime()
print >> self.f, "Result: ", euler,
self.f.flush()
with print_lock:
print euler
print "Finished Movement"
time.sleep(0.2)
saveImgEvent.clear()
if not saveImgEvent.wait(1):
break
