def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Position error
e_pos_world = self.pos_des - p
e_pos_body = transf.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = numpy.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = transf.quaternion_multiply(
transf.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(x=v_linear[0],
y=v_linear[1],
z=v_linear[2])
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def _generate_vel(self, s=None):
if self._stamped_pose_only:
return np.zeros(6)
cur_s = (self._cur_s if s is None else s)
last_s = cur_s - self.interpolator.s_step
if last_s < 0 or cur_s > 1:
return np.zeros(6)
q_cur = self.interpolator.generate_quat(cur_s)
q_last = self.interpolator.generate_quat(last_s)
cur_pos = self.interpolator.generate_pos(cur_s)
last_pos = self.interpolator.generate_pos(last_s)
########################################################
# Computing angular velocities
########################################################
# Quaternion difference to the last step in the inertial frame
q_diff = quaternion_multiply(q_cur, quaternion_inverse(q_last))
# Angular velocity
ang_vel = 2 * q_diff[0:3] / self._t_step
vel = [(cur_pos[0] - last_pos[0]) / self._t_step,
(cur_pos[1] - last_pos[1]) / self._t_step,
(cur_pos[2] - last_pos[2]) / self._t_step, ang_vel[0],
ang_vel[1], ang_vel[2]]
return np.array(vel)
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
q = msg.orientation
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def update_errors(self):
"""Update error vectors."""
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - pos)
# Update orientation error
self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat),
self._reference['rot'])
# Velocity error with respect to the the BODY frame
self._errors['vel'] = np.hstack(
(np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
if self._error_pub.get_num_connections() > 0:
stamp = self.get_clock().now().to_msg()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
pos_error = np.dot(rotBtoI, self._errors['pos'])
msg.pose.position = Vector3(x=pos_error[0],
y=pos_error[1],
z=pos_error[2])
msg.pose.orientation = Quaternion(x=self._errors['rot'][0],
y=self._errors['rot'][1],
z=self._errors['rot'][2])
# Publish velocity errors in INERTIAL frame
vel_errors = np.dot(rotBtoI, self._errors['vel'][0:3])
msg.velocity.linear = Vector3(x=vel_errors[0],
y=vel_errors[1],
z=vel_errors[2])
vel_errors = np.dot(rotBtoI, self._errors['vel'][3:6])
msg.velocity.angular = Vector3(x=vel_errors[0],
y=vel_errors[1],
z=vel_errors[2])
# msg.velocity.angular = Vector3(*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def _motion_regression_6d(self, pnts, qt, t):
"""
Compute translational and rotational velocities and accelerations in
the inertial frame at the target time t.
!!! note
[1] Sittel, Florian, Joerg Mueller, and Wolfram Burgard. Computing
velocities and accelerations from a pose time sequence in
three-dimensional space. Technical Report 272, University of
Freiburg, Department of Computer Science, 2013.
"""
lin_vel = np.zeros(3)
lin_acc = np.zeros(3)
q_d = np.zeros(4)
q_dd = np.zeros(4)
for i in range(3):
v, a = self._motion_regression_1d([(pnt['pos'][i], pnt['t'])
for pnt in pnts], t)
lin_vel[i] = v
lin_acc[i] = a
for i in range(4):
v, a = self._motion_regression_1d([(pnt['rot'][i], pnt['t'])
for pnt in pnts], t)
q_d[i] = v
q_dd[i] = a
# Keeping all velocities and accelerations in the inertial frame
ang_vel = 2 * quaternion_multiply(q_d, quaternion_conjugate(qt))
ang_acc = 2 * quaternion_multiply(q_dd, quaternion_conjugate(qt))
return np.hstack((lin_vel, ang_vel[0:3])), np.hstack(
(lin_acc, ang_acc[0:3]))
def _compute_rot_quat(self, dx, dy, dz):
if np.isclose(dx, 0) and np.isclose(dy, 0):
rotq = self._last_rot
else:
heading = np.arctan2(dy, dx)
rotq = quaternion_about_axis(heading, [0, 0, 1])
if self._is_full_dof:
rote = quaternion_about_axis(
-1 * np.arctan2(dz, np.sqrt(dx**2 + dy**2)), [0, 1, 0])
rotq = quaternion_multiply(rotq, rote)
# Certify that the next quaternion remains in the same half hemisphere
d_prod = np.dot(self._last_rot, rotq)
if d_prod < 0:
rotq *= -1
return rotq
def generate_quat(self, s):
"""Compute the quaternion of the path reference for a interpolated
point related to `s`, `s` being represented in the curve's parametric
space.
The quaternion is computed assuming the heading follows the direction
of the path towards the target. Roll and pitch can also be computed
in case the `full_dof` is set to `True`.
> *Input arguments*
* `s` (*type:* `float`): Curve's parametric input expressed in the
interval of [0, 1]
> *Returns*
Rotation quaternion as a `numpy.array` as `(x, y, z, w)`
"""
s = max(0, s)
s = min(s, 1)
last_s = s - self._s_step
if last_s == 0:
last_s = 0
if s == 0:
self._last_rot = deepcopy(self._init_rot)
return self._init_rot
this_pos = self.generate_pos(s)
last_pos = self.generate_pos(last_s)
dx = this_pos[0] - last_pos[0]
dy = this_pos[1] - last_pos[1]
dz = this_pos[2] - last_pos[2]
rotq = self._compute_rot_quat(dx, dy, dz)
self._last_rot = deepcopy(rotq)
# Calculating the step for the heading offset
q_step = quaternion_about_axis(
self._interp_fcns['heading'](s),
np.array([0, 0, 1]))
# Adding the heading offset to the rotation quaternion
rotq = quaternion_multiply(rotq, q_step)
return rotq
def odometry_callback(self, msg):
"""Handle odometry callback: The actual control loop."""
# Update local planner's vehicle position and orientation
pos = [
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
]
quat = [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
]
self.local_planner.update_vehicle_pose(pos, quat)
# Compute the desired position
t = time_in_float_sec(self.get_clock().now())
des = self.local_planner.interpolate(t)
# Publish the reference
ref_msg = TrajectoryPoint()
ref_msg.header.stamp = self.get_clock().now().to_msg()
ref_msg.header.frame_id = self.local_planner.inertial_frame_id
ref_msg.pose.position = geometry_msgs.Point(**self.to_dict_vect3(
*des.p))
ref_msg.pose.orientation = geometry_msgs.Quaternion(
**self.to_dict_quat(*des.q))
ref_msg.velocity.linear = geometry_msgs.Vector3(**self.to_dict_vect3(
*des.vel[0:3]))
ref_msg.velocity.angular = geometry_msgs.Vector3(**self.to_dict_vect3(
*des.vel[3::]))
self.reference_pub.publish(ref_msg)
p = self.vector_to_np(msg.pose.pose.position)
forward_vel = self.vector_to_np(msg.twist.twist.linear)
ref_vel = des.vel[0:3]
q = self.quaternion_to_np(msg.pose.pose.orientation)
rpy = euler_from_quaternion(q, axes='sxyz')
# Compute tracking errors wrt world frame:
e_p = des.p - p
abs_pos_error = np.linalg.norm(e_p)
abs_vel_error = np.linalg.norm(ref_vel - forward_vel)
# Generate error message
error_msg = TrajectoryPoint()
error_msg.header.stamp = self.get_clock().now().to_msg()
error_msg.header.frame_id = self.local_planner.inertial_frame_id
error_msg.pose.position = geometry_msgs.Point(**self.to_dict_vect3(
*e_p))
error_msg.pose.orientation = geometry_msgs.Quaternion(
**self.to_dict_quat(
*quaternion_multiply(quaternion_inverse(q), des.q)))
error_msg.velocity.linear = geometry_msgs.Vector3(**self.to_dict_vect3(
*(des.vel[0:3] - self.vector_to_np(msg.twist.twist.linear))))
error_msg.velocity.angular = geometry_msgs.Vector3(
**self.to_dict_vect3(
*(des.vel[3::] - self.vector_to_np(msg.twist.twist.angular))))
# Based on position tracking error: Compute desired orientation
pitch_des = -math.atan2(e_p[2], np.linalg.norm(e_p[0:2]))
# Limit desired pitch angle:
pitch_des = max(-self.desired_pitch_limit,
min(pitch_des, self.desired_pitch_limit))
yaw_des = math.atan2(e_p[1], e_p[0])
yaw_err = self.unwrap_angle(yaw_des - rpy[2])
# Limit yaw effort
yaw_err = min(self.yaw_error_limit, max(-self.yaw_error_limit,
yaw_err))
# Roll: P controller to keep roll == 0
roll_control = self.p_roll * rpy[0]
# Pitch: P controller to reach desired pitch angle
pitch_control = self.p_pitch * self.unwrap_angle(
pitch_des - rpy[1]) + self.d_pitch * (des.vel[4] -
msg.twist.twist.angular.y)
# Yaw: P controller to reach desired yaw angle
yaw_control = self.p_yaw * yaw_err + self.d_yaw * (
des.vel[5] - msg.twist.twist.angular.z)
# Limit thrust
thrust = min(
self.max_thrust,
self.p_gain_thrust * np.linalg.norm(abs_pos_error) +
self.d_gain_thrust * abs_vel_error)
thrust = max(self.min_thrust, thrust)
rpy = np.array([roll_control, pitch_control, yaw_control])
# In case the world_ned reference frame is used, the convert it back
# to the ENU convention to generate the reference fin angles
rtf = deepcopy(self.rpy_to_fins)
if self.local_planner.inertial_frame_id == 'world_ned':
rtf[:, 1] *= -1
rtf[:, 2] *= -1
# Transform orientation command into fin angle set points
fins = rtf.dot(rpy)
# Check for saturation
max_angle = max(np.abs(fins))
if max_angle >= self.max_fin_angle:
fins = fins * self.max_fin_angle / max_angle
thrust_force = self.thruster.tam_column * thrust
self.thruster.publish_command(thrust_force[0])
cmd = FloatStamped()
for i in range(self.n_fins):
cmd.header.stamp = self.get_clock().now().to_msg()
cmd.header.frame_id = '%s/fin%d' % (self.namespace, i)
cmd.data = min(fins[i], self.max_fin_angle)
cmd.data = max(cmd.data, -self.max_fin_angle)
self.pub_cmd[i].publish(cmd)
self.error_pub.publish(error_msg)
def update_errors(self):
"""Update error vectors."""
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
self.count()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
pos_veh_delay1 = self.pos_veh_prev1
pos_veh_delay2 = self.pos_veh_prev2
pos_veh_delay3 = self.pos_veh_prev3
pos_veh_delay4 = self.pos_veh_prev4
pos_veh_delay5 = self.pos_veh_prev5
pos_veh_delay6 = self.pos_veh_prev6
self.pos_veh_prev1 = self._vehicle_model.pos
self.pos_veh_prev2 = pos_veh_delay1
self.pos_veh_prev3 = pos_veh_delay2
self.pos_veh_prev4 = pos_veh_delay3
self.pos_veh_prev5 = pos_veh_delay4
self.pos_veh_prev6 = pos_veh_delay5
self.pos_veh_prev7 = pos_veh_delay6
vel_veh_delay1 = self.vel_veh_prev1
vel_veh_delay2 = self.vel_veh_prev2
vel_veh_delay3 = self.vel_veh_prev3
vel_veh_delay4 = self.vel_veh_prev4
vel_veh_delay5 = self.vel_veh_prev5
vel_veh_delay6 = self.vel_veh_prev6
self.vel_veh_prev1 = self._vehicle_model.vel
self.vel_veh_prev2 = vel_veh_delay1
self.vel_veh_prev3 = vel_veh_delay2
self.vel_veh_prev4 = vel_veh_delay3
self.vel_veh_prev5 = vel_veh_delay4
self.vel_veh_prev6 = vel_veh_delay5
self.vel_veh_prev7 = vel_veh_delay6
quat_veh_delay1 = self.quat_veh_prev1
quat_veh_delay2 = self.quat_veh_prev2
quat_veh_delay3 = self.quat_veh_prev3
quat_veh_delay4 = self.quat_veh_prev4
quat_veh_delay5 = self.quat_veh_prev5
quat_veh_delay6 = self.quat_veh_prev6
self.quat_veh_prev1 = self._vehicle_model.quat
self.quat_veh_prev2 = quat_veh_delay1
self.quat_veh_prev3 = quat_veh_delay2
self.quat_veh_prev4 = quat_veh_delay3
self.quat_veh_prev5 = quat_veh_delay4
self.quat_veh_prev6 = quat_veh_delay5
self.quat_veh_prev7 = quat_veh_delay6
# with delay
# t = rospy.get_time()
# if t>3 and t<=85:
# self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - self.pos_veh_prev2)
# self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat), self._reference['rot'])
# #self._errors['vel'] = np.hstack((np.dot(rotItoB, self._reference['vel'][0:3]) - self.vel_veh_prev2[0:3],np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# self._errors['vel'] = np.hstack((self._reference['vel'][0:3]-self.vel_veh_prev2[0:3], self._reference['vel'][3:6]-vel[3:6]))
# else:
# self._errors['pos'] = np.dot(
# rotItoB, self._reference['pos'] - pos)
# self._errors['rot'] = quaternion_multiply(
# quaternion_inverse(quat), self._reference['rot'])
# self._errors['vel'] = np.hstack((
# np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
# np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# no delay
self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - pos)
self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat),
self._reference['rot'])
self._errors['vel'] = np.hstack(
(np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# don't touch. with delay
# t = rospy.get_time()
# if t>8 and t<=85:
# self._errors['pos'] = np.dot(rotItoB, self._reference['pos'] - self.pos_veh_prev3)
# self._errors['rot'] = quaternion_multiply(quaternion_inverse(quat), self._reference['rot'])
# #self._errors['vel'] = np.hstack((np.dot(rotItoB, self._reference['vel'][0:3]) - self.vel_veh_prev2[0:3],np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# self._errors['vel'] = np.hstack((self._reference['vel'][0:3]-self.vel_veh_prev3[0:3], self._reference['vel'][3:6]-vel[3:6]))
# else:
# self._errors['pos'] = np.dot(
# rotItoB, self._reference['pos'] - pos)
# self._errors['rot'] = quaternion_multiply(
# quaternion_inverse(quat), self._reference['rot'])
# self._errors['vel'] = np.hstack((
# np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
# np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
# no delay
if self._error_pub.get_num_connections() > 0:
stamp = rospy.Time.now()
msg = TrajectoryPointObjectFollow()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(
*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(
*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
