def odom_callback(self, current_posetwist): self.lock.acquire() self.odom_active = True # Grab position; 0 Z pose = xyz_array(current_posetwist.pose.pose.position) pose[2] = 0 # Grab orientation; 0 pitch and roll orientation = numpy.array( transformations.euler_from_quaternion( xyzw_array(current_posetwist.pose.pose.orientation))) orientation[0:2] = 0 self.state = numpy.concatenate([ xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion( xyzw_array(current_posetwist.pose.pose.orientation)) ]) self.state_dot = numpy.concatenate([ xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular) ]) if (not self.desired_state_set): self.desired_state = self.state self.desired_state_set = True self.lock.release()
def right_orientation_selector(targetreses, traj_start): return sorted( targetreses, key=lambda result: _yaw_angle_between( traj_start.orientation, xyzw_array(result.pose.orientation)), reverse=True )[0]
def _odom_update(self): pos = self.last_odom_msg.pose.pose.position yaw = quat_to_rotvec( xyzw_array(self.last_odom_msg.pose.pose.orientation))[2] vel = self.last_odom_msg.twist.twist.linear dYaw = self.last_odom_msg.twist.twist.angular.z self._odom_x_label.setText('X: %3.3f' % pos.x) self._odom_y_label.setText('Y: %3.3f' % pos.y) self._odom_yaw_label.setText('Yaw: %3.3f' % yaw) self._odom_d_x_label.setText('dX: %3.3f' % vel.x) self._odom_d_y_label.setText('dY: %3.3f' % vel.y) self._odom_d_yaw_label.setText('dYaw: %3.3f' % dYaw)
def _get_target(self, result): dest_pos = xyz_array(result.pose.position) dest_orientation = xyzw_array(result.pose.orientation) # Rotate dest_orientation by 180 degrees if we're about to turn more than 90 if numpy.dot(self._traj_start.orientation, dest_orientation)**2 < .5: dest_orientation = transformations.quaternion_multiply(dest_orientation, [0, 0, 1, 0]) return self._traj_start.set_position(dest_pos) \ .set_orientation(dest_orientation) \ .height(self._traj_start.position[2]) return target
def desired_state_callback(self, desired_posetwist): self.lock.acquire() self.desired_state_set = True self.desired_state = numpy.concatenate([ xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion( xyzw_array(desired_posetwist.posetwist.pose.orientation)) ]) self.desired_state_dot = numpy.concatenate([ xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular) ]) self.lock.release()
def trajectory_callback(self, desired_trajectory): self.lock.acquire() self.desired_state_set = True # Get desired pose and orientation desired_pose = xyz_array(desired_trajectory.posetwist.pose.position) desired_orientation = transformations.euler_from_quaternion(xyzw_array(desired_trajectory.posetwist.pose.orientation)) # Get desired linear and angular velocities desired_lin_vel = xyz_array(desired_trajectory.posetwist.twist.linear) desired_ang_vel = xyz_array(desired_trajectory.posetwist.twist.angular) # Add desired position to desired state i_historys self.desired_state = numpy.concatenate([desired_pose, desired_orientation]) # Add desired velocities to velocity i_history self.desired_velocity = numpy.concatenate([desired_lin_vel, desired_ang_vel]) self.lock.release()
def desired_state_callback(desired_posetwist): global desired_state, desired_state_dot, desired_state_set lock.acquire() desired_state_set = True desired_state = numpy.concatenate([ xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion( xyzw_array(desired_posetwist.posetwist.pose.orientation)) ]) desired_state_dot = _jacobian(desired_state).dot( numpy.concatenate([ xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular) ])) lock.release()
def desired_state_callback(desired_posetwist): global desired_state, desired_state_dot, desired_state_set lock.acquire() desired_state_set = True desired_state = numpy.concatenate( [ xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion(xyzw_array(desired_posetwist.posetwist.pose.orientation)), ] ) desired_state_dot = _jacobian(desired_state).dot( numpy.concatenate( [xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular)] ) ) lock.release()
def odom_callback(self, current_posetwist): self.lock.acquire() self.odom_active = True # Grab position; 0 Z pose = xyz_array(current_posetwist.pose.pose.position) pose[2] = 0 # Grab orientation; 0 pitch and roll orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation))) orientation[0:2] = 0 self.state = numpy.concatenate([xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation))]) self.state_dot = numpy.concatenate([xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)]) if (not self.desired_state_set): self.desired_state = self.state self.desired_state_set = True self.lock.release()
def odom_callback(current_posetwist): global desired_state, desired_state_dot, state, stat_dot, state_dot_body, desired_state_set, odom_active lock.acquire() odom_active = True state = numpy.concatenate( [ xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion(xyzw_array(current_posetwist.pose.pose.orientation)), ] ) state_dot = _jacobian(state).dot( numpy.concatenate( [xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)] ) ) state_dot_body = numpy.concatenate( [xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular)] ) if not desired_state_set: desired_state = state desired_state_set = True lock.release()
def odom_callback(current_posetwist): global desired_state, desired_state_dot, state, stat_dot, state_dot_body, desired_state_set, odom_active lock.acquire() odom_active = True state = numpy.concatenate([ xyz_array(current_posetwist.pose.pose.position), transformations.euler_from_quaternion( xyzw_array(current_posetwist.pose.pose.orientation)) ]) state_dot = _jacobian(state).dot( numpy.concatenate([ xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular) ])) state_dot_body = numpy.concatenate([ xyz_array(current_posetwist.twist.twist.linear), xyz_array(current_posetwist.twist.twist.angular) ]) if (not desired_state_set): desired_state = state desired_state_set = True lock.release()
def odom_callback(self, current_pos): self.lock.acquire() self.odom_active = True # Grab current position and orientation and 0 linear Z and angluar X and Y # Get current position current_position = xyz_array(current_pos.pose.pose.position) current_orientation = numpy.array(transformations.euler_from_quaternion(xyzw_array(current_pos.pose.pose.orientation))) # Zero unneccesary elements current_position[2] = 0 current_orientation[0:2] = 0 # Add current position to state i_history self.current_state = numpy.concatenate([current_position, current_orientation]) # Get current velocities current_lin_vel = xyz_array(current_pos.twist.twist.linear) current_ang_vel = xyz_array(current_pos.twist.twist.angular) # Add current velocities to velocity i_history self.current_velocity = numpy.concatenate([current_lin_vel, current_ang_vel]) # If the desired state has not yet been set, set desired and current as the same # Resets the controller to current position on bootup if (not self.desired_state_set): self.desired_state = self.current_state self.desired_state_set = True self.lock.release()
def orientation_from_pose(p): return quat_to_rotvec(xyzw_array(p.orientation))
def desired_state_callback(self,desired_posetwist): self.lock.acquire() self.desired_state_set = True self.desired_state = numpy.concatenate([xyz_array(desired_posetwist.posetwist.pose.position), transformations.euler_from_quaternion(xyzw_array(desired_posetwist.posetwist.pose.orientation))]) self.desired_state_dot = numpy.concatenate([xyz_array(desired_posetwist.posetwist.twist.linear), xyz_array(desired_posetwist.posetwist.twist.angular)]) self.lock.release()