franka_interface/
lib.rs

1// Copyright (c) 2021 Marco Boneberger
2// Licensed under the EUPL-1.2-or-later
3use std::cell::RefCell;
4use std::convert::TryInto;
5use std::f64::consts::PI;
6use std::rc::Rc;
7use std::time::{Duration, Instant};
8pub mod experiment;
9use franka::{Frame, Matrix6x7, Matrix7, MotionGenerator};
10use nalgebra::{Isometry3, Translation3, UnitQuaternion, Vector3};
11use rubullet::{
12    BodyId, ChangeConstraintOptions, ChangeDynamicsOptions, ControlCommand, JointType,
13    PhysicsClient, UrdfOptions,
14};
15use rubullet::{ControlCommandArray, InverseKinematicsParametersBuilder, LoadModelFlags};
16
17use crate::types::{CartesianPose, GripperState, JointPositions, RobotState, Torques, Vector7};
18use std::path::PathBuf;
19
20pub(crate) mod environment;
21pub mod sim_camera;
22pub mod types;
23pub use rubullet;
24use rubullet::nalgebra::U7;
25
26/// An abstraction over a robot. Can either be a real robot or a robot in the simulation.
27#[allow(clippy::large_enum_variant)]
28pub enum Robot {
29    Sim(FrankaSim),
30    Real(FrankaReal),
31}
32impl Robot {
33    /// sets the angular damping for a simulated robot
34    pub fn set_angular_damping(&mut self, damping: f64) {
35        match self {
36            Robot::Sim(robot) => {
37                robot.physics_client.borrow_mut().change_dynamics(
38                    robot.id,
39                    None,
40                    ChangeDynamicsOptions {
41                        angular_damping: Some(damping),
42                        ..Default::default()
43                    },
44                );
45            }
46            Robot::Real(_) => {}
47        }
48    }
49    /// sets the static friction ("stiction") for a simulated robot for torque control.
50    pub fn set_joint_friction_force(&mut self, force: f64) {
51        assert!(force.is_sign_positive(), "friction force must be positive");
52        match self {
53            Robot::Sim(robot) => {
54                robot.joint_friction_force = force;
55            }
56            Robot::Real(_) => {}
57        }
58    }
59    /// point to point motion in joint space.
60    /// # Arguments
61    /// * `speed_factor` - General speed factor in range [0, 1].
62    /// * `q_goal` - Target joint positions.
63    pub fn joint_motion(&mut self, speed_factor: f64, q_goal: Vector7) {
64        match self {
65            Robot::Sim(robot) => {
66                let mut goal = [0.; 7];
67                for i in 0..7 {
68                    goal[i] = q_goal[i];
69                }
70                let mut motion_generator = MotionGenerator::new(speed_factor, &goal);
71                let control_callback = |state: &RobotState, step: &Duration| -> JointPositions {
72                    let mut franka_state = franka::RobotState::default();
73                    for i in 0..7 {
74                        franka_state.q_d[i] = state.joint_positions_d[i];
75                    }
76                    let joint_pos = motion_generator.generate_motion(&franka_state, step);
77                    joint_pos.into()
78                };
79                robot.control_joint_positions(control_callback);
80            }
81            Robot::Real(robot) => {
82                robot
83                    .robot
84                    .joint_motion(speed_factor, &q_goal.into())
85                    .unwrap();
86            }
87        }
88    }
89    /// Starts a control loop for a joint position motion generator
90    /// # Example
91    /// ```no_run
92    ///# use franka::Vector7;
93    ///# use franka_interface::experiment::{Experiment, Mode, RealSetup, SimulationSetup};
94    ///# use franka_interface::types::JointPositions;
95    ///# use franka_interface::RobotArguments;
96    ///# use std::f64::consts::PI;
97    ///# use std::path::PathBuf;
98    ///#
99    ///# struct MySimSetup {}
100    ///# impl SimulationSetup for MySimSetup {
101    ///#     fn set_franka_urdf_path(&self) -> PathBuf {
102    ///#         "path/to/panda.urdf".into()
103    ///#     }
104    ///# }
105    ///# struct MyRealSetup {}
106    ///# impl RealSetup for MyRealSetup {}
107    ///# fn main() {
108    ///#     let mut experiment = Experiment::new(Mode::Simulation, MySimSetup {}, MyRealSetup {});
109    ///#     let mut robot = experiment.new_robot(RobotArguments {
110    ///#         hostname: "franka".into(),
111    ///#         base_pose: None,
112    ///#         initial_config: None,
113    ///#     });
114    ///     let mut initial_joint_positions = Vector7::zeros();
115    ///     let mut time = 0.;
116    ///     robot.control_joint_positions(|state, time_step| {
117    ///         time += time_step.as_secs_f64();
118    ///         if time == 0. {
119    ///             initial_joint_positions = state.joint_positions;
120    ///         }
121    ///         let mut out = JointPositions::from(initial_joint_positions);
122    ///         let delta_angle = PI / 8. * (1. - f64::cos(PI / 2.5 * time));
123    ///         out.joint_positions[3] += delta_angle;
124    ///         out.joint_positions[4] += delta_angle;
125    ///         out.joint_positions[6] += delta_angle;
126    ///         if time >= 5.0 {
127    ///             println!("Finished motion, shutting down example");
128    ///             out.is_finished = true;
129    ///         }
130    ///         out
131    ///     })
132    ///# }
133    /// ```
134    pub fn control_joint_positions<F: FnMut(&RobotState, &Duration) -> JointPositions>(
135        &mut self,
136        control_callback: F,
137    ) {
138        match self {
139            Robot::Sim(robot) => {
140                robot.control_joint_positions(control_callback);
141            }
142            Robot::Real(robot) => {
143                robot.control_joint_positions(control_callback);
144            }
145        }
146    }
147
148    /// Starts a control loop for sending joint-level torque commands
149    /// # Example
150    /// ```no_run
151    ///# use franka::Vector7;
152    ///# use franka_interface::experiment::{Experiment, Mode, RealSetup, SimulationSetup};
153    ///# use franka_interface::types::Torques;
154    ///# use franka_interface::RobotArguments;
155    ///# use std::f64::consts::PI;
156    ///# use std::path::PathBuf;
157    ///# use std::time::Duration;
158    ///#
159    ///# struct MySimSetup {}
160    ///# impl SimulationSetup for MySimSetup {
161    ///#     fn set_franka_urdf_path(&self) -> PathBuf {
162    ///#         "path/to/panda.urdf".into()
163    ///#     }
164    ///#     fn set_simulation_time_step(&self) -> Duration {
165    ///#         Duration::from_millis(1)
166    ///#     }
167    ///# }
168    ///# struct MyRealSetup {}
169    ///# impl RealSetup for MyRealSetup {}
170    ///# fn main() {
171    ///#     let mut experiment = Experiment::new(Mode::Simulation, MySimSetup {}, MyRealSetup {});
172    ///#     let mut robot = experiment.new_robot(RobotArguments {
173    ///#         hostname: "franka".into(),
174    ///#         base_pose: None,
175    ///#         initial_config: None,
176    ///#     });
177    ///#
178    ///     let mut time = 0.;
179    ///     let mut rotation_force: f64 = 0.01;
180    ///     robot.control_torques(|state, time_step| {
181    ///         time += time_step.as_secs_f64();
182    ///         if state.joint_positions[6] > PI / 4. + 0.1 && rotation_force.is_sign_positive() {
183    ///             rotation_force *= -1.;
184    ///         }
185    ///         if state.joint_positions[6] < PI / 4. - 0.1 && rotation_force.is_sign_negative() {
186    ///             rotation_force *= -1.;
187    ///         }
188    ///         let mut torques = Torques::from(Vector7::from_column_slice(&[
189    ///             0.,
190    ///             0.,
191    ///             0.,
192    ///             0.,
193    ///             0.,
194    ///             0.,
195    ///             rotation_force,
196    ///         ]));
197    ///         if time > 10. {
198    ///             torques.is_finished = true;
199    ///         }
200    ///         torques
201    ///     });
202    ///# }
203    /// ```
204    pub fn control_torques<F: FnMut(&RobotState, &Duration) -> Torques>(
205        &mut self,
206        control_callback: F,
207    ) {
208        match self {
209            Robot::Sim(robot) => {
210                robot.control_torques(control_callback);
211            }
212            Robot::Real(robot) => {
213                robot.control_torques(control_callback);
214            }
215        }
216    }
217    /// Starts a control loop for a Cartesian pose motion generator
218    /// # Example
219    /// ```no_run
220    ///# use franka_interface::experiment::{Experiment, Mode, RealSetup, SimulationSetup};
221    ///# use franka_interface::types::CartesianPose;
222    ///# use franka_interface::RobotArguments;
223    ///# use nalgebra::Isometry3;
224    ///# use std::f64::consts::PI;
225    ///# use std::path::PathBuf;
226    ///# use std::time::Duration;
227    ///#
228    ///# struct MySimSetup {}
229    ///# impl SimulationSetup for MySimSetup {
230    ///#     fn set_franka_urdf_path(&self) -> PathBuf {
231    ///#          "path/to/panda.urdf".into()
232    ///#     }
233    ///#     fn set_simulation_time_step(&self) -> Duration {
234    ///#         Duration::from_millis(1)
235    ///#     }
236    ///# }
237    ///# struct MyRealSetup {}
238    ///# impl RealSetup for MyRealSetup {}
239    ///# fn main() {
240    ///#     let mut experiment = Experiment::new(Mode::Simulation, MySimSetup {}, MyRealSetup {});
241    ///#     let mut robot = experiment.new_robot(RobotArguments {
242    ///#         hostname: "franka".into(),
243    ///#         base_pose: None,
244    ///#         initial_config: None,
245    ///#     });
246    ///#
247    ///     let mut time = 0.;
248    ///     let mut initial_pose = CartesianPose::from(Isometry3::identity());
249    ///     robot.control_cartesian_pose(|state, time_step| {
250    ///         time += time_step.as_secs_f64();
251    ///         if time == 0. {
252    ///             initial_pose.pose = state.end_effector_pose;
253    ///         }
254    ///         let radius = 0.3;
255    ///         let angle = PI / 4. * (1. - f64::cos(PI / 5. * time));
256    ///         let delta_x = radius * f64::sin(angle);
257    ///         let delta_z = radius * (f64::cos(angle) - 1.);
258    ///
259    ///         let mut out = CartesianPose::from(initial_pose.pose);
260    ///         out.pose.translation.x += delta_x;
261    ///         out.pose.translation.z += delta_z;
262    ///         if time > 10. {
263    ///             out.is_finished = true;
264    ///         }
265    ///         out
266    ///     });
267    ///# }
268    /// ```
269    pub fn control_cartesian_pose<F: FnMut(&RobotState, &Duration) -> CartesianPose>(
270        &mut self,
271        control_callback: F,
272    ) {
273        match self {
274            Robot::Sim(robot) => {
275                robot.control_cartesian_pose(control_callback);
276            }
277            Robot::Real(robot) => {
278                robot.control_cartesian_pose(control_callback);
279            }
280        }
281    }
282    /// opens the gripper
283    /// # Arguments
284    /// * `width` - opening width in meter
285    pub fn open_gripper(&mut self, width: f64) {
286        match self {
287            Robot::Sim(robot) => {
288                robot.open_gripper(width);
289            }
290            Robot::Real(robot) => {
291                robot.open_gripper(width);
292            }
293        }
294    }
295    /// closes the gripper
296    pub fn close_gripper(&mut self) {
297        match self {
298            Robot::Sim(robot) => {
299                robot.close_gripper();
300            }
301            Robot::Real(robot) => {
302                robot.close_gripper();
303            }
304        }
305    }
306    /// queries the robot state
307    pub fn get_state(&mut self) -> RobotState {
308        match self {
309            Robot::Sim(robot) => robot.get_state(),
310            Robot::Real(robot) => robot.get_state(),
311        }
312    }
313    /// queries the gripper state
314    pub fn get_gripper_state(&mut self) -> GripperState {
315        match self {
316            Robot::Sim(robot) => robot.get_gripper_state(),
317            Robot::Real(robot) => robot.get_gripper_state(),
318        }
319    }
320    /// queries the current space jacobian
321    pub fn get_jacobian(&mut self) -> Matrix6x7 {
322        match self {
323            Robot::Sim(robot) => robot.get_jacobian(),
324            Robot::Real(robot) => robot.get_jacobian(),
325        }
326    }
327}
328/// a struct that specifies the argument that are needed to connect to a real robot or spawn a
329/// robot inside the simulation.
330pub struct RobotArguments {
331    /// IP address or hostname of the robot. Only needed when connected to a real robot.
332    pub hostname: String,
333    /// Base pose of the robot compared to the world frame. Only needed for the simulation.
334    pub base_pose: Option<Isometry3<f64>>,
335    /// Initial joint configuration of the robot The last two are the gripper positions.
336    /// Only needed for the simulation. Default it the home pose.
337    pub initial_config: Option<[f64; FrankaSim::PANDA_NUM_TOTAL_DOFS]>,
338}
339
340pub(crate) trait FrankaInterface {
341    fn control_joint_positions<F: FnMut(&RobotState, &Duration) -> JointPositions>(
342        &mut self,
343        control_callback: F,
344    );
345    fn control_torques<F: FnMut(&RobotState, &Duration) -> Torques>(&mut self, control_callback: F);
346    fn control_cartesian_pose<F: FnMut(&RobotState, &Duration) -> CartesianPose>(
347        &mut self,
348        control_callback: F,
349    );
350    fn open_gripper(&mut self, width: f64);
351    fn close_gripper(&mut self);
352    fn get_state(&mut self) -> RobotState;
353    fn get_gripper_state(&mut self) -> GripperState;
354    fn get_jacobian(&mut self) -> Matrix6x7;
355    fn get_jacobian_at(&mut self, q: Vector7) -> Matrix6x7;
356}
357struct Model(franka::Model);
358impl Model {
359    fn update_state_properties(&self, state: &franka::RobotState) -> RobotState {
360        let jacobian = self.0.zero_jacobian_from_state(&Frame::EndEffector, state);
361        let mass_matrix = self.0.mass_from_state(state);
362        let coriolis_force = self.0.coriolis_from_state(state);
363        let gravity = self.0.gravity_from_state(state, None);
364        let mut state: RobotState = state.clone().into();
365        state.jacobian = Matrix6x7::from_column_slice(&jacobian);
366        state.mass_matrix = Matrix7::from_column_slice(&mass_matrix);
367        state.coriolis = coriolis_force.into();
368        state.gravity = gravity.into();
369        state
370    }
371}
372/// Is responsible for the connection to the real robot.
373pub struct FrankaReal {
374    robot: franka::Robot,
375    model: Model,
376    gripper: franka::Gripper,
377}
378
379impl FrankaReal {
380    pub(crate) fn new(hostname: &str) -> Self {
381        let mut robot =
382            franka::Robot::new(hostname, None, None).expect("could not connect to robot");
383        robot
384            .set_collision_behavior(
385                [100.; 7], [100.; 7], [100.; 7], [100.; 7], [100.; 6], [100.; 6], [100.; 6],
386                [100.; 6],
387            )
388            .unwrap();
389        robot
390            .set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])
391            .unwrap();
392        robot
393            .set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])
394            .unwrap();
395        let gripper = franka::Gripper::new(hostname).expect("could not connect to gripper");
396        let model = Model(robot.load_model(false).unwrap());
397        FrankaReal {
398            robot,
399            model,
400            gripper,
401        }
402    }
403}
404
405impl FrankaInterface for FrankaReal {
406    fn control_joint_positions<F: FnMut(&RobotState, &Duration) -> JointPositions>(
407        &mut self,
408        mut control_callback: F,
409    ) {
410        let model = &self.model;
411        let new_callback =
412            |state: &franka::RobotState, duration: &Duration| -> franka::JointPositions {
413                control_callback(&model.update_state_properties(state), duration).into()
414            };
415        self.robot
416            .control_joint_positions(new_callback, None, None, None)
417            .unwrap();
418    }
419
420    fn control_torques<F: FnMut(&RobotState, &Duration) -> Torques>(
421        &mut self,
422        mut control_callback: F,
423    ) {
424        let model = &self.model;
425        let new_callback = |state: &franka::RobotState, duration: &Duration| -> franka::Torques {
426            control_callback(&model.update_state_properties(state), duration).into()
427        };
428        let robot = &mut self.robot;
429        robot
430            .set_collision_behavior(
431                [100.; 7], [100.; 7], [100.; 7], [100.; 7], [100.; 6], [100.; 6], [100.; 6],
432                [100.; 6],
433            )
434            .unwrap();
435        robot
436            .set_joint_impedance([3000., 3000., 3000., 2500., 2500., 2000., 2000.])
437            .unwrap();
438        robot
439            .set_cartesian_impedance([3000., 3000., 3000., 300., 300., 300.])
440            .unwrap();
441        robot.control_torques(new_callback, None, None).unwrap();
442    }
443
444    fn control_cartesian_pose<F: FnMut(&RobotState, &Duration) -> CartesianPose>(
445        &mut self,
446        mut control_callback: F,
447    ) {
448        let model = &self.model;
449        let new_callback =
450            |state: &franka::RobotState, duration: &Duration| -> franka::CartesianPose {
451                control_callback(&model.update_state_properties(state), duration).into()
452            };
453        self.robot
454            .control_cartesian_pose(new_callback, None, None, None)
455            .unwrap();
456    }
457
458    fn open_gripper(&mut self, width: f64) {
459        self.gripper.move_gripper(width, 0.1).unwrap();
460    }
461
462    fn close_gripper(&mut self) {
463        self.gripper.grasp(0.0, 0.1, 10., None, None).unwrap();
464    }
465
466    fn get_state(&mut self) -> RobotState {
467        let state = self.robot.read_once().unwrap();
468        self.model.update_state_properties(&state)
469    }
470
471    fn get_gripper_state(&mut self) -> GripperState {
472        self.gripper.read_once().unwrap().into()
473    }
474
475    fn get_jacobian(&mut self) -> Matrix6x7 {
476        let q = self.get_state().joint_positions;
477        self.get_jacobian_at(q)
478    }
479    fn get_jacobian_at(&mut self, q: Vector7) -> Matrix6x7 {
480        let f_t_ee = [
481            1., 0., 0., 1., 0., 1., 0., 1., 0., 0., 1., 1., 0., 0., 0.07, 1.,
482        ];
483        let jacobian =
484            self.model
485                .0
486                .zero_jacobian(&Frame::EndEffector, q.as_ref(), &f_t_ee, &[0.; 16]);
487        Matrix6x7::from_column_slice(jacobian.as_ref())
488    }
489}
490/// A struct representing a simulated Panda robot.
491pub struct FrankaSim {
492    pub(crate) physics_client: Rc<RefCell<PhysicsClient>>,
493    /// BodyId of the robot in RuBullet
494    pub id: BodyId,
495    /// the duration of a control loop cycle
496    pub time_step: Duration,
497    /// pose of the robot base relative to the world frame
498    pub base_pose: Isometry3<f64>,
499    pub joint_friction_force: f64,
500    pub robot_joint_indices: [usize; FrankaSim::PANDA_NUM_DOFS],
501    pub gripper_joint_indices: [usize; FrankaSim::GRIPPER_JOINT_NAMES.len()],
502    pub end_effector_link_index: usize,
503}
504
505impl FrankaSim {
506    const INITIAL_JOINT_POSITIONS: [f64; FrankaSim::PANDA_NUM_TOTAL_DOFS] = [
507        0.,
508        -PI / 4.,
509        0.,
510        -3. * PI / 4.,
511        0.,
512        PI / 2.,
513        PI / 4.,
514        0.02,
515        0.02,
516    ];
517    const PANDA_NUM_DOFS: usize = 7;
518    const GRIPPER_OPEN_WIDTH: f64 = 0.08;
519    const ROBOT_JOINT_NAMES: [&'static str; FrankaSim::PANDA_NUM_DOFS] = [
520        "panda_joint1",
521        "panda_joint2",
522        "panda_joint3",
523        "panda_joint4",
524        "panda_joint5",
525        "panda_joint6",
526        "panda_joint7",
527    ];
528    const GRIPPER_JOINT_NAMES: [&'static str; 2] = ["panda_finger_joint1", "panda_finger_joint2"];
529    const END_EFFECTOR_LINK_NAME: &'static str = "panda_grasptarget";
530    const PANDA_NUM_TOTAL_DOFS: usize =
531        FrankaSim::PANDA_NUM_DOFS + FrankaSim::GRIPPER_JOINT_NAMES.len();
532}
533
534pub(crate) struct FrankaSimWithoutClient {
535    pub urdf_path: PathBuf,
536    pub base_pose: Option<Isometry3<f64>>,
537    pub initial_config: Option<[f64; FrankaSim::PANDA_NUM_TOTAL_DOFS]>,
538}
539
540impl FrankaSim {
541    fn wait_for_next_cycle(&self, start_time: Instant) {
542        let time_passed = Instant::now() - start_time;
543        if time_passed < self.time_step {
544            std::thread::sleep(self.time_step - time_passed);
545        }
546    }
547    pub(crate) fn new(
548        physics_client: Rc<RefCell<PhysicsClient>>,
549        config: FrankaSimWithoutClient,
550        time_step: &Duration,
551    ) -> Self {
552        let position = config.base_pose.unwrap_or_else(|| {
553            Isometry3::rotation(
554                UnitQuaternion::from_euler_angles(-PI / 2. * 0., 0., 0.).scaled_axis(),
555            )
556        });
557        let initial_config = config
558            .initial_config
559            .unwrap_or(FrankaSim::INITIAL_JOINT_POSITIONS);
560        if let Some(directory) = config.urdf_path.parent() {
561            physics_client
562                .borrow_mut()
563                .set_additional_search_path(directory)
564                .unwrap();
565        }
566        let urdf_options = UrdfOptions {
567            use_fixed_base: true,
568            base_transform: position,
569            flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
570            ..Default::default()
571        };
572
573        let panda_id = physics_client
574            .borrow_mut()
575            .load_urdf(config.urdf_path.file_name().unwrap(), urdf_options)
576            .unwrap();
577        physics_client.borrow_mut().change_dynamics(
578            panda_id,
579            None,
580            ChangeDynamicsOptions {
581                linear_damping: Some(0.),
582                angular_damping: Some(0.),
583                ..Default::default()
584            },
585        );
586        let mut robot_joint_indices: [Option<usize>; Self::PANDA_NUM_DOFS] =
587            [None; Self::PANDA_NUM_DOFS];
588        let mut gripper_joint_indices = [None; Self::GRIPPER_JOINT_NAMES.len()];
589        let mut end_effector_link_index = None;
590        let num_joints = physics_client.borrow_mut().get_num_joints(panda_id);
591        for i in 0..num_joints {
592            let joint_info = physics_client.borrow_mut().get_joint_info(panda_id, i);
593            let joint_name = joint_info.joint_name;
594            if let Some(joint_index) = FrankaSim::ROBOT_JOINT_NAMES
595                .iter()
596                .position(|&x| x == joint_name)
597            {
598                robot_joint_indices[joint_index] = Some(i);
599            }
600            if let Some(joint_index) = FrankaSim::GRIPPER_JOINT_NAMES
601                .iter()
602                .position(|&x| x == joint_name)
603            {
604                gripper_joint_indices[joint_index] = Some(i);
605            }
606            if joint_info.link_name == FrankaSim::END_EFFECTOR_LINK_NAME {
607                end_effector_link_index = Some(i);
608            }
609        }
610        let end_effector_link_index = end_effector_link_index.unwrap_or_else(|| {
611            panic!(
612                "Could not find end-effector link: \"{}\" in URDF",
613                FrankaSim::END_EFFECTOR_LINK_NAME
614            )
615        });
616        fn turn_options_into_values<F: Fn(usize) -> String, const N: usize>(
617            array: [Option<usize>; N],
618            error_handler: F,
619        ) -> [usize; N] {
620            array
621                .iter()
622                .enumerate()
623                .map(|(i, x)| x.unwrap_or_else(|| panic!("{}", error_handler(i))))
624                .collect::<Vec<_>>()
625                .try_into()
626                .unwrap()
627        }
628
629        let robot_joint_indices = turn_options_into_values(robot_joint_indices, |i| {
630            format!(
631                "Could not find joint: \"{}\" in URDF",
632                FrankaSim::ROBOT_JOINT_NAMES[i]
633            )
634        });
635        let gripper_joint_indices = turn_options_into_values(gripper_joint_indices, |i| {
636            format!(
637                "Could not find joint: \"{}\" in URDF",
638                FrankaSim::GRIPPER_JOINT_NAMES[i]
639            )
640        });
641
642        for i in robot_joint_indices {
643            physics_client.borrow_mut().change_dynamics(
644                panda_id,
645                i,
646                ChangeDynamicsOptions {
647                    joint_damping: Some(0.0),
648                    ..Default::default()
649                },
650            );
651        }
652
653        let mut index = 0;
654        for i in robot_joint_indices {
655            let info = physics_client.borrow_mut().get_joint_info(panda_id, i);
656            if info.joint_type == JointType::Revolute || info.joint_type == JointType::Prismatic {
657                physics_client
658                    .borrow_mut()
659                    .reset_joint_state(panda_id, i, initial_config[index], None)
660                    .unwrap();
661                index += 1;
662            }
663        }
664
665        let gripper_constraint = physics_client
666            .borrow_mut()
667            .create_constraint(
668                panda_id,
669                gripper_joint_indices[0],
670                panda_id,
671                gripper_joint_indices[1],
672                JointType::Gear,
673                [1., 0., 0.],
674                Isometry3::identity(),
675                Isometry3::identity(),
676            )
677            .unwrap();
678        physics_client.borrow_mut().change_constraint(
679            gripper_constraint,
680            ChangeConstraintOptions {
681                gear_ratio: Some(-1.),
682                erp: Some(0.1),
683                max_force: Some(50.),
684                ..Default::default()
685            },
686        );
687        physics_client.borrow_mut().step_simulation().unwrap();
688        FrankaSim {
689            physics_client,
690            id: panda_id,
691            time_step: *time_step,
692            base_pose: config.base_pose.unwrap_or_else(Isometry3::identity),
693            joint_friction_force: 0.,
694            robot_joint_indices,
695            gripper_joint_indices,
696            end_effector_link_index,
697        }
698    }
699}
700
701impl FrankaInterface for FrankaSim {
702    fn control_joint_positions<F: FnMut(&RobotState, &Duration) -> JointPositions>(
703        &mut self,
704        mut control_callback: F,
705    ) {
706        let mut first_time = true;
707        loop {
708            let start_time = Instant::now();
709            let joint_positions = match first_time {
710                true => {
711                    first_time = false;
712                    control_callback(&self.get_state(), &Duration::from_secs(0))
713                }
714                false => control_callback(&self.get_state(), &self.time_step),
715            };
716
717            self.physics_client
718                .borrow_mut()
719                .set_joint_motor_control_array(
720                    self.id,
721                    &self.robot_joint_indices,
722                    ControlCommandArray::Positions(joint_positions.joint_positions.as_slice()),
723                    None,
724                )
725                .unwrap();
726            self.physics_client.borrow_mut().step_simulation().unwrap();
727            std::thread::sleep(self.time_step);
728            self.wait_for_next_cycle(start_time);
729            if joint_positions.is_finished {
730                return;
731            }
732        }
733    }
734
735    fn control_torques<F: FnMut(&RobotState, &Duration) -> Torques>(
736        &mut self,
737        mut control_callback: F,
738    ) {
739        assert!(f64::abs(self.time_step.as_secs_f64() - 0.001) < 1e-5, "the simulation time step needs to be set to 1 Millisecond to perform torque control. See set_simulation_time_step");
740        self.physics_client
741            .borrow_mut()
742            .set_joint_motor_control_array(
743                self.id,
744                &self.robot_joint_indices,
745                ControlCommandArray::Velocities(&[0.; Self::PANDA_NUM_DOFS]),
746                Some(&[self.joint_friction_force; Self::PANDA_NUM_DOFS]),
747            )
748            .unwrap();
749        let mut first_time = true;
750        loop {
751            let start_time = Instant::now();
752            let mut torques = match first_time {
753                true => {
754                    first_time = false;
755                    control_callback(&self.get_state(), &Duration::from_secs(0))
756                }
757                false => control_callback(&self.get_state(), &self.time_step),
758            };
759            let state = self.get_state();
760
761            let mut pos = [0.; FrankaSim::PANDA_NUM_TOTAL_DOFS];
762            let mut vels = [0.; FrankaSim::PANDA_NUM_TOTAL_DOFS];
763            for k in 0..Self::PANDA_NUM_DOFS {
764                pos[k] = state.joint_positions_d[k];
765                vels[k] = state.joint_velocities[k];
766            }
767
768            let gravity_torques = self
769                .physics_client
770                .borrow_mut()
771                .calculate_inverse_dynamics(
772                    self.id,
773                    &pos,
774                    &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
775                    &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
776                )
777                .unwrap();
778            let mut coriolis = self
779                .physics_client
780                .borrow_mut()
781                .calculate_inverse_dynamics(
782                    self.id,
783                    &pos,
784                    &vels,
785                    &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
786                )
787                .unwrap();
788            for i in 0..Self::PANDA_NUM_DOFS {
789                coriolis[i] -= gravity_torques[i];
790            }
791
792            torques.torques += state.gravity;
793            self.physics_client
794                .borrow_mut()
795                .set_joint_motor_control_array(
796                    self.id,
797                    &self.robot_joint_indices,
798                    ControlCommandArray::Torques(torques.torques.as_slice()),
799                    None,
800                )
801                .unwrap();
802            self.physics_client.borrow_mut().step_simulation().unwrap();
803            self.wait_for_next_cycle(start_time);
804            if torques.is_finished {
805                return;
806            }
807        }
808    }
809
810    fn control_cartesian_pose<F: FnMut(&RobotState, &Duration) -> CartesianPose>(
811        &mut self,
812        mut control_callback: F,
813    ) {
814        let mut first_time = true;
815        loop {
816            let start_time = Instant::now();
817            let pose = match first_time {
818                true => {
819                    first_time = false;
820                    control_callback(&self.get_state(), &Duration::from_secs(0))
821                }
822                false => control_callback(&self.get_state(), &self.time_step),
823            };
824            let desired_world_pose = self.base_pose * pose.pose;
825            let inverse_kinematics_parameters = InverseKinematicsParametersBuilder::new(
826                self.end_effector_link_index,
827                &desired_world_pose,
828            )
829            .set_max_num_iterations(50)
830            .build();
831            let joint_poses = self
832                .physics_client
833                .borrow_mut()
834                .calculate_inverse_kinematics(self.id, inverse_kinematics_parameters)
835                .unwrap();
836            self.physics_client
837                .borrow_mut()
838                .set_joint_motor_control_array(
839                    self.id,
840                    &self.robot_joint_indices,
841                    ControlCommandArray::Positions(&joint_poses[0..Self::PANDA_NUM_DOFS]),
842                    None,
843                )
844                .unwrap();
845            self.physics_client.borrow_mut().step_simulation().unwrap();
846            self.wait_for_next_cycle(start_time);
847            if pose.is_finished {
848                return;
849            }
850        }
851    }
852
853    fn open_gripper(&mut self, width: f64) {
854        let steps = (1. / self.time_step.as_secs_f64()) as i32;
855        let mut client = self.physics_client.borrow_mut();
856        let width = f64::clamp(width, 0., FrankaSim::GRIPPER_OPEN_WIDTH);
857        for _ in 0..steps {
858            let start_time = Instant::now();
859            client.set_joint_motor_control(
860                self.id,
861                self.gripper_joint_indices[1],
862                ControlCommand::Position(width / 2.),
863                Some(1.),
864            );
865            client.set_joint_motor_control(
866                self.id,
867                self.gripper_joint_indices[0],
868                ControlCommand::Position(width / 2.),
869                Some(1.),
870            );
871            client.step_simulation().unwrap();
872            self.wait_for_next_cycle(start_time);
873        }
874    }
875
876    fn close_gripper(&mut self) {
877        let start_width = self.get_gripper_state().gripper_width / 2.;
878        let mut client = self.physics_client.borrow_mut();
879        let steps = (1. / self.time_step.as_secs_f64()) as i32;
880        for i in 0..steps {
881            let start_time = Instant::now();
882            let val = start_width * (f64::cos((i) as f64 / steps as f64 * PI) / 2.);
883            client.set_joint_motor_control(
884                self.id,
885                self.gripper_joint_indices[1],
886                ControlCommand::Position(val),
887                Some(10.),
888            );
889            client.set_joint_motor_control(
890                self.id,
891                self.gripper_joint_indices[0],
892                ControlCommand::Position(val),
893                Some(10.),
894            );
895            client.step_simulation().unwrap();
896            self.wait_for_next_cycle(start_time);
897        }
898    }
899
900    fn get_state(&mut self) -> RobotState {
901        let indices = [
902            self.robot_joint_indices[0],
903            self.robot_joint_indices[1],
904            self.robot_joint_indices[2],
905            self.robot_joint_indices[3],
906            self.robot_joint_indices[4],
907            self.robot_joint_indices[5],
908            self.robot_joint_indices[6],
909            self.gripper_joint_indices[0],
910            self.gripper_joint_indices[1],
911        ]; // oh boy, we really need const generics in Rust.
912        let joint_states = self
913            .physics_client
914            .borrow_mut()
915            .get_joint_states(self.id, &indices)
916            .unwrap();
917        let mut joint_positions = [0.; FrankaSim::PANDA_NUM_TOTAL_DOFS];
918        let mut joint_velocities = [0.; FrankaSim::PANDA_NUM_TOTAL_DOFS];
919        for i in 0..FrankaSim::PANDA_NUM_TOTAL_DOFS {
920            joint_positions[i] = joint_states[i].joint_position;
921            joint_velocities[i] = joint_states[i].joint_velocity;
922        }
923
924        let end_effector_state = self
925            .physics_client
926            .borrow_mut()
927            .get_link_state(self.id, self.end_effector_link_index, true, true)
928            .unwrap();
929        let end_effector_pose = self.base_pose.inverse() * end_effector_state.world_pose;
930
931        let jacobian = self.get_jacobian();
932        let mass_matrix = self
933            .physics_client
934            .borrow_mut()
935            .calculate_mass_matrix(self.id, &joint_positions)
936            .unwrap();
937        let tmp = mass_matrix.fixed_slice::<U7, U7>(0, 0);
938        let mass_matrix: Matrix7 = tmp.into();
939        let coriolis_force = self
940            .physics_client
941            .borrow_mut()
942            .calculate_inverse_dynamics(
943                self.id,
944                &joint_positions,
945                &joint_velocities,
946                &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
947            )
948            .unwrap();
949
950        let gravity_torques = self
951            .physics_client
952            .borrow_mut()
953            .calculate_inverse_dynamics(
954                self.id,
955                &joint_positions,
956                &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
957                &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
958            )
959            .unwrap();
960        let gravity = Vector7::from_column_slice(&gravity_torques[0..Self::PANDA_NUM_DOFS]);
961        let coriolis_force =
962            Vector7::from_column_slice(&coriolis_force[0..Self::PANDA_NUM_DOFS]) - gravity;
963
964        let mut joint_positions = Vector7::zeros();
965        let mut joint_velocities = Vector7::zeros();
966        let mut joint_torques = Vector7::zeros();
967        for i in 0..Self::PANDA_NUM_DOFS {
968            joint_positions[i] = joint_states[i].joint_position;
969            joint_velocities[i] = joint_states[i].joint_velocity;
970            joint_torques[i] = joint_states[i].joint_motor_torque;
971        }
972        RobotState {
973            joint_positions_d: joint_positions,
974            joint_positions,
975            joint_velocities,
976            joint_velocities_d: joint_velocities,
977            joint_torques,
978            end_effector_pose_c: end_effector_pose,
979            end_effector_pose,
980            end_effector_pose_d: end_effector_pose,
981            end_effector_velocity_d: jacobian * joint_velocities,
982            jacobian,
983            mass_matrix,
984            coriolis: coriolis_force,
985            gravity,
986            control_command_success_rate: 1.,
987        }
988    }
989
990    fn get_gripper_state(&mut self) -> GripperState {
991        let width = self
992            .physics_client
993            .borrow_mut()
994            .get_joint_state(self.id, self.gripper_joint_indices[0])
995            .unwrap()
996            .joint_position
997            * 2.;
998
999        GripperState {
1000            gripper_width: width,
1001            closed: width < FrankaSim::GRIPPER_OPEN_WIDTH - 0.01,
1002        }
1003    }
1004    fn get_jacobian(&mut self) -> Matrix6x7 {
1005        let joint_states = self
1006            .physics_client
1007            .borrow_mut()
1008            .get_joint_states(self.id, &self.robot_joint_indices)
1009            .unwrap();
1010        let mut joint_positions = Vector7::zeros();
1011        for i in 0..Self::PANDA_NUM_DOFS {
1012            joint_positions[i] = joint_states[i].joint_position;
1013        }
1014        self.get_jacobian_at(joint_positions)
1015    }
1016
1017    fn get_jacobian_at(&mut self, q: Vector7) -> Matrix6x7 {
1018        let mut pos = [0.; FrankaSim::PANDA_NUM_TOTAL_DOFS];
1019        let joint_positions = q;
1020
1021        for i in 0..Self::PANDA_NUM_DOFS {
1022            pos[i] = joint_positions[i];
1023        }
1024        let jac = self
1025            .physics_client
1026            .borrow_mut()
1027            .calculate_jacobian(
1028                self.id,
1029                self.end_effector_link_index,
1030                Translation3::from(Vector3::zeros()),
1031                &pos,
1032                &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
1033                &[0.; FrankaSim::PANDA_NUM_TOTAL_DOFS],
1034            )
1035            .unwrap();
1036        Matrix6x7::from_column_slice(jac.jacobian.as_slice())
1037    }
1038}
franka_interface/lib.rs

franka_interface/
lib.rs
