diff --git a/panther_bringup/config/panther_common.yaml b/panther_bringup/config/panther_common.yaml index 4b4a3edd0..a6a3a9ae6 100644 --- a/panther_bringup/config/panther_common.yaml +++ b/panther_bringup/config/panther_common.yaml @@ -9,3 +9,17 @@ motor_torque_constant: 2.6149 gear_ratio: 30.08 encoder_resolution: 1600 # 400 * 4 robot_length: 0.44 + +# velocity smoother params +max_vel_x: 2.0 +max_vel_y: 2.0 +max_vel_theta: 4.0 +acc_lim_x: 2.7 +acc_lim_y: 2.7 +acc_lim_theta: 5.74 +decel_lim_x: 2.7 +decel_lim_y: 2.7 +decel_lim_theta: 5.74 +emergency_decel_lim_x: 2.7 +emergency_decel_lim_y: 2.7 +emergency_decel_lim_theta: 5.74 \ No newline at end of file diff --git a/panther_driver/README.md b/panther_driver/README.md index 686b36e81..6a7c1bdbd 100644 --- a/panther_driver/README.md +++ b/panther_driver/README.md @@ -53,6 +53,21 @@ For a `/joint_states` message is carrying given data: - `~wheel_radius` [*float*, default: **0.1825**]: wheel radius in **[m]**. - `~wheel_separation` [*float*, default: **0.697**]: separation of wheels alongside y axis in **[m]**. +#### Velocity smoother parameters + +- `~max_vel_x` [*float*, default: **2.0**]: maximum linear vlelocity in **X** direction. +- `~max_vel_y` [*float*, default: **2.0**]: maximum linear vlelocity in **Y** direction. +- `~max_vel_theta` [*float*, default: **4.0**]: maximum angular velocity. +- `~acc_lim_x` [*float*, default: **1.0**]: maximum linear acceleration in **X** direction. +- `~acc_lim_y` [*float*, default: **1.0**]: maximum linear acceleration in **Y** direction. +- `~acc_lim_theta` [*float*, default: **1.57**]: maximum angular acceleration. +- `~decel_lim_x` [*float*, default: **1.5**]: maximum linear decelaration in **X** direction. +- `~decel_lim_y` [*float*, default: **1.5**]: maximum linear decelaration in **Y** direction. +- `~decel_lim_theta` [*float*, default: **2.3**]: maximum angular deceleration. +- `~emergency_decel_lim_x` [*float*, default: **2.7**]: maximum linear decelaration in **X** direction when a timeout is reached for velocity commands. +- `~emergency_decel_lim_y` [*float*, default: **2.7**]: maximum linear decelaration in **Y** direction when a timeout is reached for velocity commands. +- `~emergency_decel_lim_theta` [*float*, default: **5.74**]: maximum angular deceleration when a timeout is reached for velocity commands. + #### Kinematics type - explanation The Panther robot can be configured with different wheels to match your needs, we provide 2 different kinematics types `differential`/`mecanum`. You can change the wheel type by providing an appropriate launch parameter with a path to the wheel configuration file - `wheel_config_file`. Basic wheel configuration files (*WH01.yaml, WH02.yaml, WH04.yaml*): are located in `panther_description` package. diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 2932a6947..f0ce09ce0 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -17,6 +17,7 @@ from panther_msgs.msg import DriverState, FaultFlag, IOState, RuntimeError, ScriptFlag from panther_can import PantherCANSDO, PantherCANPDO from panther_kinematics import PantherDifferential, PantherMecanum +from velocity_smoother import VelocitySmoother class DriverFlagLogger: @@ -87,6 +88,19 @@ def __init__(self, name: str) -> None: robot_length = rospy.get_param('~robot_length', 0.44) wheel_radius = rospy.get_param('~wheel_radius', 0.1825) + max_vel_x = rospy.get_param('~max_vel_x', 2.0) + max_vel_y = rospy.get_param('~max_vel_y', 2.0) + max_vel_theta = rospy.get_param('~max_vel_theta', 4.0) + acc_lim_x = rospy.get_param('~acc_lim_x', 1.0) + acc_lim_y = rospy.get_param('~acc_lim_y', 1.0) + acc_lim_theta = rospy.get_param('~acc_lim_theta', 1.57) + decel_lim_x = rospy.get_param('~decel_lim_x', 1.5) + decel_lim_y = rospy.get_param('~decel_lim_y', 1.5) + decel_lim_theta = rospy.get_param('~decel_lim_theta', 2.3) + emergency_decel_lim_x = rospy.get_param('~emergency_decel_lim_x', 2.7) + emergency_decel_lim_y = rospy.get_param('~emergency_decel_lim_y', 2.7) + emergency_decel_lim_theta = rospy.get_param('~emergency_decel_lim_theta', 5.74) + self._wheels_joints_names = [ 'fl_wheel_joint', 'fr_wheel_joint', @@ -111,7 +125,22 @@ def __init__(self, name: str) -> None: self._e_stop_cliented = False self._motor_on = False - self._stop_cmd_vel_cb = True + self._stop_motors = True + + self._velocity_smoother = VelocitySmoother( + max_vel_x, + max_vel_y, + max_vel_theta, + acc_lim_x, + acc_lim_y, + acc_lim_theta, + decel_lim_x, + decel_lim_y, + decel_lim_theta, + emergency_decel_lim_x, + emergency_decel_lim_y, + emergency_decel_lim_theta, + ) # ------------------------------- # Kinematic type @@ -269,12 +298,18 @@ def _main_timer_cb(self, *args) -> None: dt = (time_now - self._time_last).to_sec() self._time_last = time_now - if (time_now - self._cmd_vel_command_last_time) < rospy.Duration( - secs=self._cmd_vel_timeout - ): - self._panther_can.write_wheels_enc_velocity( - self._panther_kinematics.wheels_enc_speed + if (time_now - self._cmd_vel_command_last_time) > rospy.Duration(self._cmd_vel_timeout): + self._cmd_vel = Twist() + emergency_breaking = True + else: + emergency_breaking = False + + if not self._stop_motors: + cmd_vel = self._velocity_smoother.get_smooth_velocity( + self._cmd_vel, self._main_timer_period, emergency_breaking ) + self._panther_kinematics.inverse_kinematics(cmd_vel) + self._panther_can.write_wheels_enc_velocity(self._panther_kinematics.wheels_enc_speed) else: self._panther_can.write_wheels_enc_velocity([0.0, 0.0, 0.0, 0.0]) @@ -361,7 +396,7 @@ def _safety_timer_cb(self, *args) -> None: if any(self._panther_can.can_connection_error()): if not e_stop_cliented: self._trigger_panther_e_stop() - self._stop_cmd_vel_cb = True + self._stop_motors = True if not self._motor_on: rospy.logwarn_throttle( @@ -377,21 +412,16 @@ def _safety_timer_cb(self, *args) -> None: f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure)', ) elif self._e_stop_cliented: - self._stop_cmd_vel_cb = True + self._stop_motors = True else: - self._stop_cmd_vel_cb = False + self._stop_motors = False def _e_stop_cb(self, data: Bool) -> None: with self._lock: self._e_stop_cliented = data.data def _cmd_vel_cb(self, data: Twist) -> None: - # Block all motors if any Roboteq controller returns a fault flag or runtime error flag - if not self._stop_cmd_vel_cb: - self._panther_kinematics.inverse_kinematics(data) - else: - self._panther_kinematics.inverse_kinematics(Twist()) - + self._cmd_vel = data self._cmd_vel_command_last_time = rospy.Time.now() def _io_state_cb(self, io_state: IOState) -> None: diff --git a/panther_driver/src/velocity_smoother.py b/panther_driver/src/velocity_smoother.py new file mode 100755 index 000000000..c8e9d6175 --- /dev/null +++ b/panther_driver/src/velocity_smoother.py @@ -0,0 +1,96 @@ +#!/usr/bin/python3 + +from geometry_msgs.msg import Twist + + +class VelocitySmoother: + def __init__( + self, + max_vel_x, + max_vel_y, + max_vel_theta, + acc_lim_x, + acc_lim_y, + acc_lim_theta, + decel_lim_x, + decel_lim_y, + decel_lim_theta, + emergency_decel_lim_x, + emergency_decel_lim_y, + emergency_decel_lim_theta, + ) -> None: + + self._max_vel_x = max_vel_x + self._max_vel_y = max_vel_y + self._max_vel_theta = max_vel_theta + self._acc_lim_x = acc_lim_x + self._acc_lim_y = acc_lim_y + self._acc_lim_theta = acc_lim_theta + self._decel_lim_x = decel_lim_x + self._decel_lim_y = decel_lim_y + self._decel_lim_theta = decel_lim_theta + self._emergency_decel_lim_x = emergency_decel_lim_x + self._emergency_decel_lim_y = emergency_decel_lim_y + self._emergency_decel_lim_theta = emergency_decel_lim_theta + + self._robot_vel_x = 0.0 + self._robot_vel_y = 0.0 + self._robot_vel_theta = 0.0 + + def get_smooth_velocity( + self, cmd_vel: Twist, period: float, emergency_breaking: bool = False + ) -> Twist: + acc_x_factor = self._acc_lim_x * period + acc_y_factor = self._acc_lim_y * period + acc_theta_factor = self._acc_lim_theta * period + if not emergency_breaking: + decel_x_factor = self._decel_lim_x * period + decel_y_factor = self._decel_lim_y * period + decel_theta_factor = self._decel_lim_theta * period + else: + decel_x_factor = self._emergency_decel_lim_x * period + decel_y_factor = self._emergency_decel_lim_y * period + decel_theta_factor = self._emergency_decel_lim_theta * period + + output_vel = Twist() + output_vel.linear.x = self._clamp( + self._accel_limiter(cmd_vel.linear.x, self._robot_vel_x, acc_x_factor, decel_x_factor), + -self._max_vel_x, + self._max_vel_x, + ) + output_vel.linear.y = self._clamp( + self._accel_limiter(cmd_vel.linear.y, self._robot_vel_y, acc_y_factor, decel_y_factor), + -self._max_vel_y, + self._max_vel_y, + ) + output_vel.angular.z = self._clamp( + self._accel_limiter( + cmd_vel.angular.z, + self._robot_vel_theta, + acc_theta_factor, + decel_theta_factor, + ), + -self._max_vel_theta, + self._max_vel_theta, + ) + + self._robot_vel_x = output_vel.linear.x + self._robot_vel_y = output_vel.linear.y + self._robot_vel_theta = output_vel.angular.z + + return output_vel + + def _accel_limiter( + self, cmd_vel: float, robot_vel: float, acc_factor: float, decel_factor + ) -> float: + # When changing the sign of the command velocity, the robot should stop + # (approach a velocity of 0.0) by applying a deceleration factor. + # Then, it should use acceleration factor to reach the desired velocity value. + if robot_vel >= 0.0: + return robot_vel + self._clamp(cmd_vel - robot_vel, -decel_factor, acc_factor) + else: + return robot_vel + self._clamp(cmd_vel - robot_vel, -acc_factor, decel_factor) + + @staticmethod + def _clamp(value, min_value, max_value): + return max(min(value, max_value), min_value)