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