openrr_base/types/
differential_drive.rs

Help
1use arci::{BaseVelocity, Localization, MotorDriveVelocity, MoveBase};
2
3use crate::*;
4
5#[derive(Debug)]
6pub struct DifferentialDriveHardwareParameters {
7    pub wheel_radius: f64,
8    pub tread_width: f64,
9}
10
11#[derive(Debug)]
12pub struct DifferentialDriveMotorController<MV: MotorDriveVelocity> {
13    pub left: MV,
14    pub right: MV,
15}
16
17#[derive(Debug)]
18pub struct DifferentialDrive<MV>
19where
20    MV: MotorDriveVelocity,
21{
22    robot_velocity: RobotVelocityStatus,
23    controller: DifferentialDriveMotorController<MV>,
24    param: DifferentialDriveHardwareParameters,
25    odometry: Odometry,
26}
27
28impl<MV> DifferentialDrive<MV>
29where
30    MV: MotorDriveVelocity,
31{
32    pub fn new(
33        param: DifferentialDriveHardwareParameters,
34        controller: DifferentialDriveMotorController<MV>,
35        limit_velocity: BaseVelocity,
36        limit_acceleration: BaseAcceleration,
37    ) -> Self {
38        Self {
39            robot_velocity: RobotVelocityStatus::new(limit_velocity, limit_acceleration),
40            controller,
41            param,
42            odometry: Odometry::default(),
43        }
44    }
45
46    /// Output: left_wheel_velocity, right_wheel_velocity
47    pub fn transform_velocity_base_to_wheel(&self, velocity: &BaseVelocity) -> [f64; 2] {
48        [
49            (velocity.x - 0.5 * self.param.tread_width * velocity.theta) / self.param.wheel_radius,
50            (velocity.x + 0.5 * self.param.tread_width * velocity.theta) / self.param.wheel_radius,
51        ]
52    }
53
54    /// Input: left_wheel_velocity, right_wheel_velocity
55    pub fn transform_velocity_wheel_to_base(&self, wheels_vel: &[f64; 2]) -> BaseVelocity {
56        let left = wheels_vel[0];
57        let right = wheels_vel[1];
58        let translation = 0.5 * self.param.wheel_radius * (left + right);
59        let rotation = self.param.wheel_radius * (right - left) / self.param.tread_width;
60        BaseVelocity {
61            x: translation,
62            y: 0f64,
63            theta: rotation,
64        }
65    }
66}
67
68impl<MV> MoveBase for DifferentialDrive<MV>
69where
70    MV: MotorDriveVelocity,
71{
72    fn send_velocity(&self, velocity: &BaseVelocity) -> Result<(), arci::Error> {
73        let limited_vel = self.robot_velocity.get_limited_velocity(velocity);
74
75        let wheels_vel = self.transform_velocity_base_to_wheel(&limited_vel);
76
77        self.controller
78            .left
79            .set_motor_velocity(wheels_vel[0])
80            .unwrap();
81        self.controller
82            .right
83            .set_motor_velocity(wheels_vel[1])
84            .unwrap();
85
86        let wheels_feedback = [
87            self.controller.left.get_motor_velocity().unwrap(),
88            self.controller.right.get_motor_velocity().unwrap(),
89        ];
90        let feedback_vel = self.transform_velocity_wheel_to_base(&wheels_feedback);
91
92        match self.odometry.update_by_velocity(&feedback_vel) {
93            Ok(_) => {}
94            Err(e) => println!("{e}"),
95        }
96
97        self.robot_velocity.set_log(&limited_vel);
98
99        Ok(())
100    }
101
102    fn current_velocity(&self) -> Result<BaseVelocity, arci::Error> {
103        let left_wheel_rotation = self.controller.left.get_motor_velocity().unwrap();
104        let right_wheel_rotation = self.controller.right.get_motor_velocity().unwrap();
105
106        let feedback_vel =
107            self.transform_velocity_wheel_to_base(&[left_wheel_rotation, right_wheel_rotation]);
108
109        self.robot_velocity.set_velocity_state(feedback_vel);
110
111        Ok(feedback_vel)
112    }
113}
114
115impl<MV> Localization for DifferentialDrive<MV>
116where
117    MV: MotorDriveVelocity,
118{
119    fn current_pose(&self, _frame_id: &str) -> Result<arci::Isometry2<f64>, arci::Error> {
120        match self.odometry.current_pose() {
121            Ok(pose) => Ok(pose),
122            Err(e) => Err(arci::Error::Other(e.into())),
123        }
124    }
125}
126
127#[cfg(test)]
128mod test {
129    use arci::DummyMotorDriveVelocity;
130    use assert_approx_eq::assert_approx_eq;
131
132    use super::*;
133
134    const LIMIT_VEL_X: f64 = 10.0;
135    const LIMIT_VEL_THETA: f64 = 10.0;
136    const LIMIT_ACC_X: f64 = 10.0;
137    const LIMIT_ACC_THETA: f64 = 10.0;
138
139    #[test]
140    fn test_velocity_transformer() {
141        let param = DifferentialDriveHardwareParameters {
142            wheel_radius: 0.5,
143            tread_width: 1.0,
144        };
145        let controller = DifferentialDriveMotorController {
146            left: DummyMotorDriveVelocity::default(),
147            right: DummyMotorDriveVelocity::default(),
148        };
149
150        controller.left.set_motor_velocity(1.234).unwrap();
151        controller.right.set_motor_velocity(2.345).unwrap();
152
153        let limit_velocity = BaseVelocity::new(LIMIT_VEL_X, 0.0, LIMIT_VEL_THETA);
154        let limit_acceleration = BaseAcceleration::new(LIMIT_ACC_X, 0.0, LIMIT_ACC_THETA);
155
156        let dd = DifferentialDrive::new(param, controller, limit_velocity, limit_acceleration);
157
158        let straight_vel = BaseVelocity {
159            x: 1.0,
160            y: 0.0,
161            theta: 0.0,
162        };
163        let turning_vel = BaseVelocity {
164            x: 0.0,
165            y: 0.0,
166            theta: 1.0,
167        };
168
169        let straight_wheel_vel = dd.transform_velocity_base_to_wheel(&straight_vel);
170        let turning_wheel_vel = dd.transform_velocity_base_to_wheel(&turning_vel);
171
172        assert_approx_eq!(straight_wheel_vel[0], 2.0);
173        assert_approx_eq!(straight_wheel_vel[1], 2.0);
174        assert_approx_eq!(turning_wheel_vel[0], -1.0);
175        assert_approx_eq!(turning_wheel_vel[1], 1.0);
176    }
177}
openrr_base/types/differential_drive.rs

openrr_base/types/
differential_drive.rs
