openrr_client/clients/
collision_avoidance_client.rs

Help
1use std::{path::Path, sync::Arc};
2
3use arci::{Error, JointTrajectoryClient, TrajectoryPoint, WaitFuture};
4use openrr_planner::{create_joint_path_planner, JointPathPlannerConfig};
5
6// TODO: speed limit
7fn trajectory_from_positions(
8    positions: &[Vec<f64>],
9    total_duration: std::time::Duration,
10) -> Vec<TrajectoryPoint> {
11    let num_points = positions.len();
12    let mut traj = vec![];
13    for (i, pos) in positions.iter().enumerate() {
14        let time_rate: f64 = ((i + 1) as f64) / (num_points as f64);
15        traj.push(TrajectoryPoint::new(
16            pos.clone(),
17            total_duration.mul_f64(time_rate),
18        ));
19    }
20    traj
21}
22
23pub struct CollisionAvoidanceClient<T>
24where
25    T: JointTrajectoryClient,
26{
27    pub client: T,
28    pub planner: openrr_planner::JointPathPlanner<f64>,
29}
30
31impl<T> CollisionAvoidanceClient<T>
32where
33    T: JointTrajectoryClient,
34{
35    pub fn new(client: T, planner: openrr_planner::JointPathPlanner<f64>) -> Self {
36        Self { client, planner }
37    }
38}
39
40impl<T> JointTrajectoryClient for CollisionAvoidanceClient<T>
41where
42    T: JointTrajectoryClient,
43{
44    fn joint_names(&self) -> Vec<String> {
45        self.client.joint_names()
46    }
47
48    fn current_joint_positions(&self) -> Result<Vec<f64>, Error> {
49        self.client.current_joint_positions()
50    }
51
52    fn send_joint_positions(
53        &self,
54        positions: Vec<f64>,
55        duration: std::time::Duration,
56    ) -> Result<WaitFuture, Error> {
57        let traj = self
58            .planner
59            .plan_avoid_self_collision(
60                &self.joint_names(),
61                &self.current_joint_positions()?,
62                &positions,
63            )
64            .map_err(|e| Error::Other(e.into()))?;
65        self.client
66            .send_joint_trajectory(trajectory_from_positions(&traj, duration))
67    }
68
69    fn send_joint_trajectory(&self, trajectory: Vec<TrajectoryPoint>) -> Result<WaitFuture, Error> {
70        if trajectory.is_empty() {
71            return Ok(WaitFuture::ready());
72        }
73        let positions = self
74            .planner
75            .plan_avoid_self_collision(
76                &self.joint_names(),
77                &self.current_joint_positions()?,
78                &trajectory[0].positions,
79            )
80            .map_err(|e| Error::Other(e.into()))?;
81        let mut trajs = trajectory_from_positions(&positions, trajectory[0].time_from_start);
82
83        for i in 1..trajectory.len() {
84            let positions = self
85                .planner
86                .plan_avoid_self_collision(
87                    &self.joint_names(),
88                    &trajectory[i - 1].positions,
89                    &trajectory[i].positions,
90                )
91                .map_err(|e| Error::Other(e.into()))?;
92            trajs.append(&mut trajectory_from_positions(
93                &positions,
94                trajectory[i].time_from_start,
95            ));
96        }
97        self.client.send_joint_trajectory(trajs)
98    }
99}
100
101pub fn create_collision_avoidance_client<P: AsRef<Path>>(
102    urdf_path: P,
103    self_collision_check_pairs: Vec<(String, String)>,
104    joint_path_planner_config: &JointPathPlannerConfig,
105    client: Arc<dyn JointTrajectoryClient>,
106    reference_robot: Arc<k::Chain<f64>>,
107) -> CollisionAvoidanceClient<Arc<dyn JointTrajectoryClient>> {
108    let planner = create_joint_path_planner(
109        urdf_path,
110        self_collision_check_pairs,
111        joint_path_planner_config,
112        reference_robot,
113    );
114
115    CollisionAvoidanceClient::new(client, planner)
116}
117
118#[cfg(test)]
119mod tests {
120    use super::*;
121
122    #[tokio::test]
123    async fn test_create_collision_avoidance_client() {
124        let urdf_path = Path::new("sample.urdf");
125        let urdf_robot = urdf_rs::read_file(urdf_path).unwrap();
126        let robot = Arc::new(k::Chain::<f64>::from(&urdf_robot));
127        let client = arci::DummyJointTrajectoryClient::new(
128            robot
129                .iter_joints()
130                .map(|joint| joint.name.clone())
131                .collect(),
132        );
133        client
134            .send_joint_positions(vec![0.0; 8], std::time::Duration::new(0, 0))
135            .unwrap()
136            .await
137            .unwrap();
138
139        let collision_avoidance_client = create_collision_avoidance_client(
140            urdf_path,
141            vec![("root".to_owned(), "l_shoulder_roll".to_owned())],
142            &JointPathPlannerConfig::default(),
143            Arc::new(client),
144            robot,
145        );
146
147        assert_eq!(
148            *collision_avoidance_client
149                .current_joint_positions()
150                .unwrap(),
151            vec![0.0; 8]
152        );
153
154        // No collision case
155        assert!(collision_avoidance_client
156            .send_joint_positions(vec![0.0; 8], std::time::Duration::new(1, 0),)
157            .is_ok());
158        // Collision occurs in the reference position
159        assert!(collision_avoidance_client
160            .send_joint_positions(
161                vec![1.57, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
162                std::time::Duration::new(1, 0),
163            )
164            .is_err());
165    }
166}
openrr_client/clients/collision_avoidance_client.rs

openrr_client/clients/
collision_avoidance_client.rs
