libfranka  0.14.1
FCI C++ API
control_types.h
Go to the documentation of this file.
1 // Copyright (c) 2023 Franka Robotics GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <array>
6 #include <cmath>
7 #include <initializer_list>
8 
14 namespace franka {
15 
19 enum class ControllerMode { kJointImpedance, kCartesianImpedance };
20 
26 enum class RealtimeConfig { kEnforce, kIgnore };
27 
35 struct Finishable {
39  bool motion_finished = false;
40 };
41 
45 class Torques : public Finishable {
46  public:
52  Torques(const std::array<double, 7>& torques) noexcept;
53 
61  Torques(std::initializer_list<double> torques);
62 
66  std::array<double, 7> tau_J{}; // NOLINT(readability-identifier-naming)
67 };
68 
72 class JointPositions : public Finishable {
73  public:
79  JointPositions(const std::array<double, 7>& joint_positions) noexcept;
80 
88  JointPositions(std::initializer_list<double> joint_positions);
89 
93  std::array<double, 7> q{};
94 };
95 
99 class JointVelocities : public Finishable {
100  public:
107  JointVelocities(const std::array<double, 7>& joint_velocities) noexcept;
108 
116  JointVelocities(std::initializer_list<double> joint_velocities);
117 
121  std::array<double, 7> dq{};
122 };
123 
127 class CartesianPose : public Finishable {
128  public:
136  CartesianPose(const std::array<double, 16>& cartesian_pose) noexcept;
137 
146  CartesianPose(const std::array<double, 16>& cartesian_pose,
147  const std::array<double, 2>& elbow) noexcept;
148 
158  CartesianPose(std::initializer_list<double> cartesian_pose);
159 
171  CartesianPose(std::initializer_list<double> cartesian_pose, std::initializer_list<double> elbow);
172 
178  std::array<double, 16> O_T_EE{}; // NOLINT(readability-identifier-naming)
179 
193  std::array<double, 2> elbow{};
194 
201  bool hasElbow() const noexcept;
202 };
203 
212  public:
220  CartesianVelocities(const std::array<double, 6>& cartesian_velocities) noexcept;
221 
230  CartesianVelocities(const std::array<double, 6>& cartesian_velocities,
231  const std::array<double, 2>& elbow) noexcept;
232 
242  CartesianVelocities(std::initializer_list<double> cartesian_velocities);
243 
254  CartesianVelocities(std::initializer_list<double> cartesian_velocities,
255  std::initializer_list<double> elbow);
256 
261  std::array<double, 6> O_dP_EE{}; // NOLINT(readability-identifier-naming)
262 
275  std::array<double, 2> elbow{};
276 
282  bool hasElbow() const noexcept;
283 };
284 
294 inline Torques MotionFinished(Torques command) noexcept { // NOLINT(readability-identifier-naming)
295  command.motion_finished = true;
296  return command;
297 }
298 
308 inline JointPositions MotionFinished( // NOLINT(readability-identifier-naming)
309  JointPositions command) noexcept {
310  command.motion_finished = true;
311  return command;
312 }
313 
323 inline JointVelocities MotionFinished( // NOLINT(readability-identifier-naming)
324  JointVelocities command) noexcept {
325  command.motion_finished = true;
326  return command;
327 }
328 
338 inline CartesianPose MotionFinished( // NOLINT(readability-identifier-naming)
339  CartesianPose command) noexcept {
340  command.motion_finished = true;
341  return command;
342 }
343 
353 inline CartesianVelocities MotionFinished( // NOLINT(readability-identifier-naming)
354  CartesianVelocities command) noexcept {
355  command.motion_finished = true;
356  return command;
357 }
358 
359 } // namespace franka
Stores values for Cartesian pose motion generation.
Definition: control_types.h:127
CartesianPose(const std::array< double, 16 > &cartesian_pose, const std::array< double, 2 > &elbow) noexcept
Creates a new CartesianPose instance.
std::array< double, 16 > O_T_EE
Homogeneous transformation , column major, that transforms from the end effector frame to base frame...
Definition: control_types.h:178
CartesianPose(const std::array< double, 16 > &cartesian_pose) noexcept
Creates a new CartesianPose instance.
bool hasElbow() const noexcept
Determines whether there is a stored elbow configuration.
CartesianPose(std::initializer_list< double > cartesian_pose)
Creates a new CartesianPose instance.
CartesianPose(std::initializer_list< double > cartesian_pose, std::initializer_list< double > elbow)
Creates a new CartesianPose instance.
std::array< double, 2 > elbow
Elbow configuration.
Definition: control_types.h:193
Stores values for Cartesian velocity motion generation.
Definition: control_types.h:211
CartesianVelocities(std::initializer_list< double > cartesian_velocities)
Creates a new CartesianVelocities instance.
bool hasElbow() const noexcept
Determines whether there is a stored elbow configuration.
CartesianVelocities(const std::array< double, 6 > &cartesian_velocities) noexcept
Creates a new CartesianVelocities instance.
CartesianVelocities(const std::array< double, 6 > &cartesian_velocities, const std::array< double, 2 > &elbow) noexcept
Creates a new CartesianVelocities instance.
CartesianVelocities(std::initializer_list< double > cartesian_velocities, std::initializer_list< double > elbow)
Creates a new CartesianVelocities instance.
Stores values for joint position motion generation.
Definition: control_types.h:72
JointPositions(std::initializer_list< double > joint_positions)
Creates a new JointPositions instance.
std::array< double, 7 > q
Desired joint angles in [rad].
Definition: control_types.h:93
JointPositions(const std::array< double, 7 > &joint_positions) noexcept
Creates a new JointPositions instance.
Stores values for joint velocity motion generation.
Definition: control_types.h:99
JointVelocities(const std::array< double, 7 > &joint_velocities) noexcept
Creates a new JointVelocities instance.
std::array< double, 7 > dq
Desired joint velocities in .
Definition: control_types.h:121
JointVelocities(std::initializer_list< double > joint_velocities)
Creates a new JointVelocities instance.
Stores joint-level torque commands without gravity and friction.
Definition: control_types.h:45
Torques(const std::array< double, 7 > &torques) noexcept
Creates a new Torques instance.
Torques(std::initializer_list< double > torques)
Creates a new Torques instance.
std::array< double, 7 > tau_J
Desired torques in [Nm].
Definition: control_types.h:66
Torques MotionFinished(Torques command) noexcept
Helper method to indicate that a motion should stop after processing the given command.
Definition: control_types.h:294
ControllerMode
Available controller modes for a franka::Robot.
Definition: control_types.h:19
RealtimeConfig
Used to decide whether to enforce realtime mode for a control loop thread.
Definition: control_types.h:26
Helper type for control and motion generation loops.
Definition: control_types.h:35
bool motion_finished
Determines whether to finish a currently running motion.
Definition: control_types.h:39