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)