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()
def orientation_from_pose(p): return quat_to_rotvec(xyzw_array(p.orientation))
