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 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 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)