openrr_teleop/
ik.rs

1use std::{
2    sync::{Arc, Mutex},
3    time::Duration,
4};
5
6use arci::{
7    gamepad::{Axis, Button, GamepadEvent},
8    JointTrajectoryClient, Speaker,
9};
10use async_trait::async_trait;
11use k::{Translation3, Vector3};
12use openrr_client::IkSolverWithChain;
13use schemars::JsonSchema;
14use serde::{Deserialize, Serialize};
15
16use super::control_mode::ControlMode;
17
18const IK_POSITION_TURBO_GAIN: f64 = 2.0;
19
20struct IkModeInner {
21    linear_velocity: Vector3<f64>,
22    angular_velocity: Vector3<f64>,
23    move_step_linear: [f64; 3],
24    move_step_angular: [f64; 3],
25    is_turbo: bool,
26    is_sending: bool,
27}
28
29impl IkModeInner {
30    fn new(move_step_linear: [f64; 3], move_step_angular: [f64; 3]) -> Self {
31        Self {
32            linear_velocity: Vector3::new(0.0, 0.0, 0.0),
33            angular_velocity: Vector3::new(0.0, 0.0, 0.0),
34            move_step_linear,
35            move_step_angular,
36            is_turbo: false,
37            is_sending: false,
38        }
39    }
40
41    fn handle_event(&mut self, event: GamepadEvent) {
42        match event {
43            GamepadEvent::ButtonPressed(Button::LeftTrigger2) => {
44                self.is_turbo = true;
45            }
46            GamepadEvent::ButtonReleased(Button::LeftTrigger2) => {
47                self.is_turbo = false;
48            }
49            GamepadEvent::ButtonPressed(Button::RightTrigger2) => {
50                self.is_sending = true;
51            }
52            GamepadEvent::ButtonReleased(Button::RightTrigger2) => {
53                self.is_sending = false;
54                self.clear_velocity();
55            }
56            GamepadEvent::ButtonPressed(Button::South) => {
57                self.linear_velocity.z = -self.move_step_linear[2];
58            }
59            GamepadEvent::ButtonReleased(Button::South) => {
60                self.linear_velocity.z = 0.0;
61            }
62            GamepadEvent::ButtonPressed(Button::West) => {
63                self.linear_velocity.z = self.move_step_linear[2];
64            }
65            GamepadEvent::ButtonReleased(Button::West) => {
66                self.linear_velocity.z = 0.0;
67            }
68            GamepadEvent::AxisChanged(Axis::RightStickY, v) => {
69                self.linear_velocity.x = self.move_step_linear[0] * v;
70            }
71            GamepadEvent::AxisChanged(Axis::RightStickX, v) => {
72                self.linear_velocity.y = self.move_step_linear[1] * v;
73            }
74            GamepadEvent::AxisChanged(Axis::LeftStickX, v) => {
75                self.angular_velocity.x = -self.move_step_angular[0] * v;
76            }
77            GamepadEvent::AxisChanged(Axis::LeftStickY, v) => {
78                self.angular_velocity.y = self.move_step_angular[1] * v;
79            }
80            GamepadEvent::ButtonPressed(Button::DPadRight) => {
81                self.angular_velocity.z = -self.move_step_angular[2];
82            }
83            GamepadEvent::ButtonReleased(Button::DPadRight) => {
84                self.angular_velocity.z = 0.0;
85            }
86            GamepadEvent::ButtonPressed(Button::DPadLeft) => {
87                self.angular_velocity.z = self.move_step_angular[2];
88            }
89            GamepadEvent::ButtonReleased(Button::DPadLeft) => {
90                self.angular_velocity.z = 0.0;
91            }
92            GamepadEvent::Disconnected => {
93                self.is_sending = false;
94                self.is_turbo = false;
95                self.clear_velocity();
96            }
97            _ => {}
98        }
99    }
100
101    fn clear_velocity(&mut self) {
102        self.linear_velocity.x = 0.0;
103        self.linear_velocity.y = 0.0;
104        self.linear_velocity.z = 0.0;
105        self.angular_velocity.x = 0.0;
106        self.angular_velocity.y = 0.0;
107        self.angular_velocity.z = 0.0;
108    }
109
110    fn get_linear_velocity(&self) -> Vector3<f64> {
111        self.linear_velocity
112            * if self.is_turbo {
113                IK_POSITION_TURBO_GAIN
114            } else {
115                1.0
116            }
117    }
118}
119
120pub struct IkMode<J, S>
121where
122    J: JointTrajectoryClient,
123    S: Speaker,
124{
125    joint_trajectory_client: J,
126    speaker: S,
127    mode: String,
128    submode: String,
129    step_duration: Duration,
130    ik_solver_with_chain: Arc<IkSolverWithChain>,
131    inner: Mutex<IkModeInner>,
132}
133
134impl<J, S> IkMode<J, S>
135where
136    J: JointTrajectoryClient,
137    S: Speaker,
138{
139    pub fn new(
140        mode: String,
141        joint_trajectory_client: J,
142        move_step_linear: [f64; 3],
143        move_step_angular: [f64; 3],
144        step_duration: Duration,
145        speaker: S,
146        ik_solver_with_chain: Arc<IkSolverWithChain>,
147    ) -> Self {
148        Self {
149            joint_trajectory_client,
150            speaker,
151            mode,
152            submode: "".to_string(),
153            step_duration,
154            ik_solver_with_chain,
155            inner: Mutex::new(IkModeInner::new(move_step_linear, move_step_angular)),
156        }
157    }
158
159    pub fn new_from_config(
160        config: IkModeConfig,
161        joint_trajectory_client: J,
162        speaker: S,
163        ik_solver_with_chain: Arc<IkSolverWithChain>,
164    ) -> Self {
165        Self::new(
166            config.mode,
167            joint_trajectory_client,
168            config.move_step_linear,
169            config.move_step_angular,
170            Duration::from_secs_f64(config.step_duration_secs),
171            speaker,
172            ik_solver_with_chain,
173        )
174    }
175}
176
177#[async_trait]
178impl<N, S> ControlMode for IkMode<N, S>
179where
180    N: JointTrajectoryClient,
181    S: Speaker,
182{
183    fn handle_event(&self, event: GamepadEvent) {
184        self.inner.lock().unwrap().handle_event(event);
185    }
186
187    async fn proc(&self) {
188        let (is_sending, angular_velocity, linear_velocity) = {
189            let inner = self.inner.lock().unwrap();
190            (
191                inner.is_sending,
192                inner.angular_velocity,
193                inner.get_linear_velocity(),
194            )
195        };
196        if is_sending {
197            let current_positions = self
198                .joint_trajectory_client
199                .current_joint_positions()
200                .unwrap();
201            self.ik_solver_with_chain
202                .set_joint_positions_clamped(&current_positions);
203            let current_pose = self.ik_solver_with_chain.end_transform();
204            let rotated = current_pose
205                * k::UnitQuaternion::from_euler_angles(
206                    angular_velocity.x,
207                    angular_velocity.y,
208                    angular_velocity.z,
209                );
210            let target_pose = rotated * Translation3::from(linear_velocity);
211            if self.ik_solver_with_chain.solve(&target_pose).is_ok() {
212                let pos = self.ik_solver_with_chain.joint_positions();
213                self.joint_trajectory_client
214                    .send_joint_positions(pos, self.step_duration)
215                    .unwrap()
216                    .await
217                    .unwrap();
218            } else {
219                self.speaker.speak("ik fail").unwrap().await.unwrap();
220            }
221        }
222    }
223
224    fn mode(&self) -> &str {
225        &self.mode
226    }
227
228    fn submode(&self) -> String {
229        self.submode.to_owned()
230    }
231}
232
233#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
234#[serde(deny_unknown_fields)]
235pub struct IkModeConfig {
236    pub mode: String,
237    #[serde(default = "default_move_step_angular")]
238    pub move_step_angular: [f64; 3],
239    #[serde(default = "default_move_step_linear")]
240    pub move_step_linear: [f64; 3],
241    #[serde(default = "default_step_duration_secs")]
242    pub step_duration_secs: f64,
243}
244
245const fn default_move_step_angular() -> [f64; 3] {
246    [0.05, 0.05, 0.17]
247}
248
249const fn default_move_step_linear() -> [f64; 3] {
250    [0.01, 0.01, 0.01]
251}
252
253const fn default_step_duration_secs() -> f64 {
254    0.1
255}
256
257#[cfg(test)]
258mod tests {
259    use assert_approx_eq::*;
260
261    use super::*;
262
263    #[test]
264    fn test_default_move_step_angular() {
265        let def = default_move_step_angular();
266
267        assert_eq!(def.len(), 3_usize);
268        assert_approx_eq!(def[0], 0.05_f64);
269        assert_approx_eq!(def[1], 0.05_f64);
270        assert_approx_eq!(def[2], 0.17_f64);
271    }
272
273    #[test]
274    fn test_default_move_step_linear() {
275        let def = default_move_step_linear();
276
277        assert_eq!(def.len(), 3_usize);
278        assert_approx_eq!(def[0], 0.01_f64);
279        assert_approx_eq!(def[1], 0.01_f64);
280        assert_approx_eq!(def[2], 0.01_f64);
281    }
282
283    #[test]
284    fn test_default_step_duration_secs() {
285        let def = default_step_duration_secs();
286
287        assert_approx_eq!(def, 0.1_f64);
288    }
289}