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)