franka_interface/
types.rs

1// Copyright (c) 2021 Marco Boneberger
2// Licensed under the EUPL-1.2-or-later
3//! Contains the control types and types for the robot and gripper state
4pub use franka::Matrix7;
5pub use franka::Vector7;
6use franka::{Finishable, Matrix6x7};
7use nalgebra::{Isometry3, Vector6};
8
9/// Describes the State of the Robot at a certain point in time.
10#[derive(Debug)]
11pub struct RobotState {
12    /// desired join positions
13    pub joint_positions_d: Vector7,
14    /// measured joint positions
15    pub joint_positions: Vector7,
16    /// measured joint velocities
17    pub joint_velocities: Vector7,
18    /// desired joint velocities
19    pub joint_velocities_d: Vector7,
20    /// measured joint torques
21    pub joint_torques: Vector7,
22    /// command end-effector pose. Use it cartesian pose control
23    pub end_effector_pose_c: Isometry3<f64>,
24    /// measured end-effector pose.
25    pub end_effector_pose: Isometry3<f64>,
26    /// desired end-effector pose
27    pub end_effector_pose_d: Isometry3<f64>,
28    /// desired end-effector velocity (linear,angular)
29    pub end_effector_velocity_d: Vector6<f64>,
30    /// Jacobian at the current configuration
31    pub jacobian: Matrix6x7,
32    /// Mass matrix at the current configuration
33    pub mass_matrix: Matrix7,
34    /// Coriolis torques at the current configuration
35    pub coriolis: Vector7,
36    /// Gravity torques at the current configuration
37    pub gravity: Vector7,
38    /// Percentage of the last 100 robot commands that the robot successfully received
39    pub control_command_success_rate: f64,
40}
41/// Describes the state of the Gripper at a certain point in time.
42pub struct GripperState {
43    /// current gripper width in meter
44    pub gripper_width: f64,
45    /// whether the gripper grasped an object.
46    pub closed: bool,
47}
48/// Describes the desired Torques that will be send to the robot.
49pub struct Torques {
50    /// desired joint torques
51    pub torques: Vector7,
52    /// whether the robot should stop the control loop after this message.
53    pub is_finished: bool,
54}
55
56impl From<franka::Torques> for Torques {
57    fn from(t: franka::Torques) -> Self {
58        let is_finished = t.is_finished();
59        Torques {
60            torques: Vector7::from_column_slice(&t.tau_J),
61            is_finished,
62        }
63    }
64}
65
66impl From<Torques> for franka::Torques {
67    fn from(t: Torques) -> Self {
68        let is_finished = t.is_finished;
69        let mut out = franka::Torques::new(t.torques.into());
70        out.set_motion_finished(is_finished);
71        out
72    }
73}
74
75impl From<Vector7> for Torques {
76    fn from(vec: Vector7) -> Self {
77        Torques {
78            torques: vec,
79            is_finished: false,
80        }
81    }
82}
83/// Describes the desired joint positions that will be send to the robot.
84pub struct JointPositions {
85    /// desired joint positions
86    pub joint_positions: Vector7,
87    /// whether the robot should stop the control loop after this message.
88    pub is_finished: bool,
89}
90
91impl From<franka::JointPositions> for JointPositions {
92    fn from(t: franka::JointPositions) -> Self {
93        let is_finished = t.is_finished();
94        JointPositions {
95            joint_positions: Vector7::from_column_slice(&t.q),
96            is_finished,
97        }
98    }
99}
100
101impl From<JointPositions> for franka::JointPositions {
102    fn from(t: JointPositions) -> Self {
103        let is_finished = t.is_finished;
104        let mut out = franka::JointPositions::new(t.joint_positions.into());
105        out.set_motion_finished(is_finished);
106        out
107    }
108}
109
110impl From<Vector7> for JointPositions {
111    fn from(vec: Vector7) -> Self {
112        JointPositions {
113            joint_positions: vec,
114            is_finished: false,
115        }
116    }
117}
118/// Describes the desired cartesian pose that will be send to the robot.
119pub struct CartesianPose {
120    /// desired end-effector pose
121    pub pose: Isometry3<f64>,
122    /// whether the robot should stop the control loop after this message.
123    pub is_finished: bool,
124}
125
126impl From<franka::CartesianPose> for CartesianPose {
127    fn from(pose: franka::CartesianPose) -> Self {
128        CartesianPose {
129            pose: franka::utils::array_to_isometry(&pose.O_T_EE),
130            is_finished: pose.is_finished(),
131        }
132    }
133}
134
135impl From<CartesianPose> for franka::CartesianPose {
136    fn from(pose: CartesianPose) -> Self {
137        let is_finished = pose.is_finished;
138        let mut out: franka::CartesianPose = pose.pose.into();
139        out.set_motion_finished(is_finished);
140        out
141    }
142}
143
144impl From<Isometry3<f64>> for CartesianPose {
145    fn from(pose: Isometry3<f64>) -> Self {
146        CartesianPose {
147            pose,
148            is_finished: false,
149        }
150    }
151}
152
153impl From<franka::RobotState> for RobotState {
154    fn from(state: franka::RobotState) -> Self {
155        RobotState {
156            joint_positions_d: Vector7::from_column_slice(&state.q_d),
157            joint_positions: Vector7::from_column_slice(&state.q),
158            joint_velocities: Vector7::from_column_slice(&state.dq),
159            joint_velocities_d: Vector7::from_column_slice(&state.dq),
160            joint_torques: Vector7::from_column_slice(&state.tau_J),
161            end_effector_pose_c: franka::utils::array_to_isometry(&state.O_T_EE_c),
162            end_effector_pose_d: franka::utils::array_to_isometry(&state.O_T_EE_d),
163            end_effector_pose: franka::utils::array_to_isometry(&state.O_T_EE),
164            end_effector_velocity_d: state.O_dP_EE_d.into(),
165            jacobian: Matrix6x7::zeros(),
166            mass_matrix: Matrix7::zeros(),
167            coriolis: Vector7::zeros(),
168            gravity: Vector7::zeros(),
169            control_command_success_rate: state.control_command_success_rate,
170        }
171    }
172}
173
174impl From<franka::GripperState> for GripperState {
175    fn from(state: franka::GripperState) -> Self {
176        GripperState {
177            gripper_width: state.width,
178            closed: state.is_grasped,
179        }
180    }
181}
182
183#[cfg(test)]
184mod tests {
185    use crate::types::{Torques, Vector7};
186
187    #[test]
188    pub fn convert_torques() {
189        let vec = Vector7::from_column_slice(&[0., 1., 2., 3., 4., 5., 6.]);
190        let torque = Torques::from(vec.clone());
191        let franka_torque: franka::Torques = torque.into();
192        let orig_torque: Torques = franka_torque.into();
193        let orig_vec: Vector7 = orig_torque.torques;
194        for i in 0..7 {
195            assert_eq!(orig_vec[i], vec[i]);
196        }
197    }
198}
franka_interface/types.rs

franka_interface/
types.rs
