def computeOdom(self): while True: self.computeSpeed() xdot = (1 / 2) * math.cos(self.theta) * self.speedRight + ( 1 / 2) * math.cos(self.theta) * self.speedLeft ydot = (1 / 2) * math.sin(self.theta) * self.speedRight + ( 1 / 2) * math.sin(self.theta) * self.speedLeft thetadot = (1.0 / WHEEL_BASE) * self.speedRight - ( 1.0 / WHEEL_BASE) * self.speedLeft self.x = self.x + 0.05 * xdot self.y = self.y + 0.05 * ydot self.theta = self.theta + 0.05 * thetadot q = euler2quat(0.0, 0.0, self.theta) odom = Odometry() h = Header() h.stamp = rospy.Time.now() h.frame_id = 'odom' odom.header = h odom.child_frame_id = 'base_link' odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.orientation.x = q[1] odom.pose.pose.orientation.y = q[2] odom.pose.pose.orientation.z = q[3] odom.pose.pose.orientation.w = q[0] self.odomPub.publish(odom) time.sleep(0.05)
def load_Y_values(csv_filename): ''' Given a csv file, will return a Y matrix containing all the pose information associated with each training example as well as a list of filenames ''' with open(csv_filename, newline='') as csvfile: filenames = [] reader = csv.reader(csvfile) data = list(reader)[1:] file_examples = [] # for each image for i in range(len(data)): list_of_params = data[i][1].split() examples = [] k = 0 # for each car in the image while k < len(list_of_params): pose = list_of_params[k+1:k+7] pose = [float(i) for i in pose] translation = pose[3:] eulers = pose[:3] # parameters are ordered roll, pitch, yaw (input dataset is yaw, pitch, roll) quaternion_rot = list(euler2quat(eulers[2], eulers[1], eulers[0])) examples.append(quaternion_rot + translation) k += 7 examples = sorted(examples,key=lambda x: x[5]) file_examples.append(examples) filenames.append(str(data[i][0]) + '.jpg') return file_examples, filenames
def create_model_state(self, name, pos, orientation_euler): model_state = ModelState() model_state.model_name = name model_state.pose.position = pos quat = euler2quat(0, orientation_euler, 0) model_state.pose.orientation.x = quat[3] model_state.pose.orientation.y = quat[1] model_state.pose.orientation.z = quat[2] model_state.pose.orientation.w = quat[0] return model_state
def apply_transform(self): quat_rot = squaternion.euler2quat(*self.rotation, True) glTranslatef(*self.transform) rot_angle = np.degrees(2 * np.arccos(quat_rot.w)) square_coef = np.sqrt(1 - np.square(quat_rot.w)) if rot_angle > 0.001 and square_coef > 0.001: rot_x = quat_rot.x / square_coef rot_y = quat_rot.y / square_coef rot_z = quat_rot.z / square_coef glRotatef(rot_angle, rot_x, rot_y, rot_z) glScalef(*self.scale)
def refresh_pos(self): if self.rot_xy: quat_rot = squaternion.euler2quat(self.rot_xy[1], self.rot_xy[0], 0, True) rot_angle = np.degrees(2 * np.arccos(quat_rot.w)) square_coef = np.sqrt(1 - np.square(quat_rot.w)) if rot_angle > 0.001 and square_coef > 0.001: rot_x = quat_rot.x / square_coef rot_y = quat_rot.y / square_coef rot_z = quat_rot.z / square_coef glRotatef(rot_angle, rot_x, rot_y, rot_z) glTranslatef(*[-coor for coor in self.transform]) glTranslatef(*[-coor for coor in self.pos])
def compensate(self, accel, mag): """ """ try: mx, my, mz = self.normalize(*mag) # ax, ay, az = self.grav(*accel) ax, ay, az = self.normalize(*accel) pitch = asin(-ax) if abs(pitch) >= pi / 2: roll = 0.0 else: roll = asin(ay / cos(pitch)) # mx, my, mz = mag x = mx * cos(pitch) + mz * sin(pitch) y = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin( roll) * cos(pitch) heading = atan2(y, x) # wrap heading between 0 and 360 degrees if heading > 2 * pi: heading -= 2 * pi elif heading < 0: heading += 2 * pi if self.angle_units == self.DEGREES: roll *= 180 / pi pitch *= 180 / pi heading *= 180 / pi elif self.angle_units == self.QUATERNIONS: return euler2quat(roll, pitch, heading) return ( roll, pitch, heading, ) except ZeroDivisionError as e: print('Error', e) if self.angle_units == self.QUATERNIONS: return Quaternion(1, 0, 0, 0) else: return ( 0.0, 0.0, 0.0, )
def read_bno(): """Function to read the BNO sensor and update the bno_data object with the latest BNO orientation, etc. state. Must be run in its own thread because it will never return!""" ser = serial.Serial('COM4', 9600) time.sleep(2) while True: serRead = str(ser.readline()) #read from serial port serRead = serRead.split(' | ') #split the string into parts at '|' tmilli = serRead[0].split('|') degx, degy, degz = 0, 0, 0 if len(tmilli) == 2: tmilli = tmilli[1] if len(serRead) == 8: #Make sure we're reading a complete line AcX = serRead[1].split(' = ') AcY = serRead[2].split(' = ') AcZ = serRead[3].split(' = ') Tmp = serRead[4].split(' = ') GyX = serRead[5].split(' = ') #roll GyY = serRead[6].split(' = ') #pitch GyZ = serRead[7].split(' = ') #yaw GyZ = GyZ[1].split('\\') #remove \\r\\n at end of line AcX = int(AcX[1]) / 16384 AcY = int(AcY[1]) / 16384 AcZ = int(AcZ[1]) / 16384 GyX = int(GyX[1]) / 131 GyY = int(GyY[1]) / 131 GyZ = int(GyZ[0]) / 131 degx = ((math.atan(float(AcY) / math.sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / math.pi)-.58)*0.04 + (degx + (GyX-6.8) * (int(tmilli)/1000))*.96 degy = ((math.atan(-1 * AcX / math.sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / math.pi)+1.58)*0.04 + (degy + (GyY-2.5) * (int(tmilli)/1000))*.96 degz = degz + (GyZ-0.24)*(int(tmilli)/1000) #Inputs are ANGLES not angular acceleration #BELOW INPUTS ARE euler2quat(roll, pitch, yaw, degrees=False), default is in radians...send true if in degrees q = euler2quat(degx, degz, degy, True) #typecast, scale and convert euler angles to quaternion vector # Capture the lock on the bno_changed condition so the bno_data shared # state can be updated. with bno_changed: bno_data["euler"] = [GyZ, GyY, GyX] bno_data["temp"] = float(Tmp[1]) bno_data["quaternion"] = [q[1], q[2],q[3], q[0]] bno_data["calibration"] = [int(3), int(3), int(3), int(3)] #"3 indicates fully calibrated; 0 indicates not calibrated" Page 68 BNO055 bno_data["acceleration"] = [AcX, AcY, AcZ] # Notify any waiting threads that the BNO state has been updated. bno_changed.notifyAll() print("notified all") # Sleep until the next reading. time.sleep(1.0 / BNO_UPDATE_FREQUENCY_HZ)
def getAllChildren(startPoint): for o in startPoint: if o.get('type') == 'JOINT': matrix = list(map(float, o[0].text.split())) #pos = list(map(float, o[0].text.split())) posX = float(matrix[3]) posY = float(matrix[7]) posZ = float(matrix[11]) scaleX = 1 scaleY = 1 scaleZ = 1 #pos = list(map(float, o[4].text.split())) #rot = list(map(str, o[1].text.split())) #rot2 = list(map(float, o[2].text.split())) #rot3 = list(map(float, o[3].text.split())) #rotx = math.ceil(float(math.cos(matrix[5]) + math.sin(-matrix[6]) + math.sin(matrix[9]) + math.cos(matrix[10]))) #roty = math.ceil(float(math.cos(matrix[0]) + math.sin(-matrix[2]) + math.sin(matrix[8]) + math.cos(matrix[10]))) #rotz = math.ceil(float(math.cos(matrix[0]) + math.sin(-matrix[1]) + math.sin(matrix[4]) + math.cos(matrix[5]))) rotx = matrix[0] + matrix[4] + matrix[8] roty = matrix[1] + matrix[5] + matrix[9] rotz = matrix[2] + matrix[6] + matrix[10] print(rotx, ' ', roty, ' ', rotz) q = euler2quat(rotz, roty, rotx, degrees=True) print(q[1], ' ', q[2], ' ', q[3], ' ', q[0]) #print(scale) #rint(parent_map[o].get('sid')) if parent_map[o].get('type') == 'NODE': classic = parent_map[o].get('name') else: classic = parent_map[o].get('sid') tempArray = [ o.get('sid'), classic, q[1], q[2], q[3], q[0], posX, posY, posZ, scaleX, scaleY, scaleZ ] fullNodeArray.append(tempArray) getAllChildren(o)
while True: if tracking: #bajamos la velocidad rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_x", "0.3") rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_x_backwards", "0.15") rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_y", "0.15") rospy.set_param("/robot/move_base/TebLocalPlannerROS/max_vel_theta", "0.3") rospy.set_param("/robot/move_base/TebLocalPlannerROS/acc_lim_y", "0.05") rospy.set_param("/robot/move_base/TebLocalPlannerROS/acc_lim_x", "0.05") goal = [] goal.append(calculate_goal_position(robot, person, SECURITY_RADIUS)) # index 0 q = euler2quat(0, 0, goal[0].theta) # theta en rad gp = MoveBaseGoal() gp.target_pose.header.stamp = rospy.Time.now() gp.target_pose.header.frame_id = "robot_map" gp.target_pose.pose.position.x = goal[0].x gp.target_pose.pose.position.y = goal[0].y gp.target_pose.pose.position.z = goal[0].z gp.target_pose.pose.orientation.x = q.x gp.target_pose.pose.orientation.y = q.y gp.target_pose.pose.orientation.z = q.z gp.target_pose.pose.orientation.w = q.w print ("GOAL: x = " + str(goal[0].x) + " y = " + str(goal[0].y)) print ("ROBOT: x = " + str(robot.x) + " y = " + str(robot.y))
msg = Imu() while not geckopy.is_shutdown(): a, m, g = imu.get() msg.linear_acceleration.x = a[0] msg.linear_acceleration.y = a[1] msg.linear_acceleration.z = a[2] msg.angular_velocity.x = g[0] msg.angular_velocity.y = g[1] msg.angular_velocity.z = g[2] msg.magnetic_field.x = m[0] msg.magnetic_field.y = m[1] msg.magnetic_field.z = m[2] r, p, h = imu.getOrientation(a, m) q = euler2quat(r, p, h, degrees=True) msg.orientation.w = q.w msg.orientation.x = q.x msg.orientation.y = q.y msg.orientation.z = q.z print(msg) pub.publish(protobufPack(msg)) rate.sleep()
if if_comm: conn.setinit() obs = env.reset() env_obj = env.unwrapped while True: action = policy.act(obs, env) obs, r, done, _ = env.step(action) if freq_count == frequency: x, y = conn.gettarget() env_obj.set_flag(x, y) freq_count = 0 pos = env_obj.body_xyz quat = np.array(euler2quat(*env_obj.body_rpy)) pad = np.append(pos, quat) joints = np.array([]) for j in env_obj.jdict: joints = np.append(joints, env_obj.jdict[j].current_position()) joints = joints[0::2] remote_data = np.concatenate([pad] + [joints]) if if_comm: conn.setqpos(remote_data) score += r frame += 1 if if_render: env.render("human") freq_count += 1