Skip to content
Snippets Groups Projects
Commit a5a72276 authored by Florian Walch's avatar Florian Walch
Browse files

Merge pull request #44 in SWDEV/franka_ros from...

Merge pull request #44 in SWDEV/franka_ros from feature/SWDEV-58-ros-enable-doxygen-for-ros-packages to master

* commit 'e0edfcaa7cc4ffecec8002d145ea3555aabd602e':
  Fixes for clang-format.
  franka_control: Resolve Doxygen warnings.
  franka_hw: Improve docu, resolve Doxygen warnings.
  Include basic Doxygen config in all packages.
parents 3740063d 9b43fa42
No related branches found
No related tags found
No related merge requests found
Showing
with 330 additions and 198 deletions
......@@ -15,58 +15,40 @@
namespace franka_control {
/**
* Controller to publish the robot state as ROS topics.
*/
class FrankaStateController
: public controller_interface::MultiInterfaceController<franka_hw::FrankaStateInterface> {
public:
/**
* Creates an instance of a FrankaStateController.
*/
FrankaStateController();
/**
* Initializes the controller with interfaces and publishers
* Initializes the controller with interfaces and publishers.
*
* @param[in] hardware Pointer to the robot hardware
* @param[in] root_node_handle Nodehandle on root level passed from HW node
* @param[in] controller_node_handle Nodehandle in the controller namespace
* passed from HW node
* @param[in] robot_hardware RobotHW instance to get a franka_hw::FrankaStateInterface from.
* @param[in] root_node_handle Node handle in the controller_manager namespace.
* @param[in] controller_node_handle Node handle in the controller namespace.
*/
bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& controller_node_handle) override;
/**
* Reads a new franka robot state and publishes it
* Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it.
*
* @param[in] time Current ros time
* @param[in] time Current ROS time.
* @param[in] period Time since the last update.
*/
void update(const ros::Time& time, const ros::Duration&) override;
void update(const ros::Time& time, const ros::Duration& period) override;
private:
/**
* Publishes all relevant data received from the Franka arm
*
* @param[in] time Current ros time
*/
void publishFrankaStates(const ros::Time& time);
/**
* Publishes the joint states of the Franka arm
*
* @param[in] time Current ros time
*/
void publishJointStates(const ros::Time& time);
/**
* Publishes the transforms for EE and K frame which define the end-effector
* (EE) and the Cartesian impedance reference frame (K)
*
* @param[in] time Current ros time
*/
void publishTransforms(const ros::Time& time);
/**
* Publishes the estimated external wrench felt by the Franka
*
* @param[in] time Current ros time
*/
void publishExternalWrench(const ros::Time& time);
std::string arm_id_;
......
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for FRANKA EMIKA research robots: https://frankaemika.github.io
*/
- builder: doxygen
javadoc_autobrief: YES
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for FRANKA EMIKA research robots: https://frankaemika.github.io
*/
- builder: doxygen
javadoc_autobrief: YES
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for FRANKA EMIKA research robots: https://frankaemika.github.io
*/
- builder: doxygen
javadoc_autobrief: YES
#pragma once
#include <array>
#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/internal/hardware_resource_manager.h>
#include <string>
namespace franka_hw {
/** \brief A handle used to read and command a Cartesian Pose to a Franka. */
/**
* Handle to read and command a Cartesian pose.
*/
class FrankaCartesianPoseHandle : public FrankaStateHandle {
public:
FrankaCartesianPoseHandle() = delete;
/**
* \param cartesian_state_handle The cartesian state handle
* \param command A reference to the storage field for the cartesian pose
* passed a desired homogeneous transformation O_T_EE_d
* Creates an instance of a FrankaCartesianPoseHandle.
*
* @param[in] franka_state_handle Robot state handle.
* @param[in] command A reference to the Cartesian pose command wrapped by this handle.
*/
FrankaCartesianPoseHandle(const FrankaStateHandle& franka_state_handle,
std::array<double, 16>& command)
: FrankaStateHandle(franka_state_handle), command_(&command) {}
void setCommand(const std::array<double, 16>& command) { *command_ = command; }
const std::array<double, 16>& getCommand() const { return *command_; }
/**
* Sets the given command.
*
* @param[in] command Command to set.
*/
void setCommand(const std::array<double, 16>& command) noexcept { *command_ = command; }
/**
* Gets the current command.
*
* @return Current command.
*/
const std::array<double, 16>& getCommand() const noexcept { return *command_; }
private:
std::array<double, 16>* command_;
};
/** \brief Hardware interface to support commanding Cartesian poses to a Franka.
/**
* Hardware interface to command Cartesian poses.
*/
class FrankaPoseCartesianInterface
: public hardware_interface::HardwareResourceManager<FrankaCartesianPoseHandle,
hardware_interface::ClaimResources> {};
/** \brief A handle used to read and command a Cartesian Velocity to a Franka.
/**
* Handle to read and command a Cartesian velocity.
*/
class FrankaCartesianVelocityHandle : public FrankaStateHandle {
public:
FrankaCartesianVelocityHandle() = delete;
/**
* \param cartesian_state_handle The cartesian state handle
* \param command A reference to the storage field for the cartesian velocity
* passed a desired homogeneous transformation O_T_EE_d
* @param[in] franka_state_handle Robot state handle.
* @param[in] command A reference to the Cartesian velocity command wrapped by this handle.
*/
FrankaCartesianVelocityHandle(const FrankaStateHandle& franka_state_handle,
std::array<double, 6>& command)
: FrankaStateHandle(franka_state_handle), command_(&command) {}
void setCommand(std::array<double, 6>& command) { *command_ = command; }
const std::array<double, 6>& getCommand() const { return *command_; }
/**
* Sets the given command.
*
* @param[in] command Command to set.
*/
void setCommand(std::array<double, 6>& command) noexcept { *command_ = command; }
/**
* Gets the current command.
*
* @return Current command.
*/
const std::array<double, 6>& getCommand() const noexcept { return *command_; }
private:
std::array<double, 6>* command_;
};
/** \brief Hardware interface to support commanding Cartesian velocities to a
* Franka
/**
* Hardware interface to command Cartesian velocities.
*/
class FrankaVelocityCartesianInterface
: public hardware_interface::HardwareResourceManager<FrankaCartesianVelocityHandle,
......
......@@ -25,16 +25,27 @@ namespace franka_hw {
class FrankaHW : public hardware_interface::RobotHW {
public:
/**
* Maximum allowed joint acceleration.
*
* Used to limit commanded joint positions, velocities, and torques.
*/
static constexpr double kMaximumJointAcceleration{1.0};
/**
* Maximum allowed joint jerk.
*
* Used to limit commanded joint positions, velocities, and torques.
*/
static constexpr double kMaximumJointJerk{4000.0};
FrankaHW() = delete;
/**
* Constructs an instance of FrankaHW that does not provide a model interface.
* Creates an instance of FrankaHW that does not provide a model interface.
*
* @param[in] joint_names An array of joint names being controlled.
* @param[in] arm_id Unique identifier for the FRANKA arm being controlled.
* @param[in] arm_id Unique identifier for the robot being controlled.
* @param[in] node_handle A node handle to get parameters from.
*/
FrankaHW(const std::array<std::string, 7>& joint_names,
......@@ -42,12 +53,12 @@ class FrankaHW : public hardware_interface::RobotHW {
const ros::NodeHandle& node_handle);
/**
* Constructs an instance of FrankaHW that provides a model interface.
* Creates an instance of FrankaHW that provides a model interface.
*
* @param[in] joint_names An array of joint names being controlled.
* @param[in] arm_id Unique identifier for the FRANKA arm being controlled.
* @param[in] arm_id Unique identifier for the robot being controlled.
* @param[in] node_handle A node handle to get parameters from.
* @param[in] model FRANKA model.
* @param[in] model Robot model.
*/
FrankaHW(const std::array<std::string, 7>& joint_names,
const std::string& arm_id,
......@@ -59,20 +70,18 @@ class FrankaHW : public hardware_interface::RobotHW {
/**
* Runs the currently active controller in a realtime loop.
*
* If no controller is active, the function immediately exits.
* When running a controller, the function only exits when ros_callback
* returns false.
* If no controller is active, the function immediately exits. When running a controller,
* the function only exits when ros_callback returns false.
*
* @param[in] robot Robot instance.
* @param[in] ros_callback A callback function that is executed at each time
* step and runs all ROS-side functionality of the hardware. Execution is
* stopped if it returns false.
*
* @throw franka::ControlException if an error related to control occurred.
* @throw franka::NetworkException if the connection is lost, e.g. after a
* timeout.
* @throw franka::RealtimeException if realtime priority can not be set for
* the current thread.
* @param[in] ros_callback A callback function that is executed at each time step and runs
* all ROS-side functionality of the hardware. Execution is stopped if it returns false.
*
* @throw franka::ControlException if an error related to torque control or motion generation
* occurred.
* @throw franka::InvalidOperationException if a conflicting operation is already running.
* @throw franka::NetworkException if the connection is lost, e.g. after a timeout.
* @throw franka::RealtimeException if realtime priority cannot be set for the current thread.
*/
void control(franka::Robot& robot,
std::function<bool(const ros::Time&, const ros::Duration&)> ros_callback);
......@@ -82,6 +91,7 @@ class FrankaHW : public hardware_interface::RobotHW {
*
* @param[in] robot_state Current robot state.
*
* @throw franka::InvalidOperationException if a conflicting operation is already running.
* @throw franka::NetworkException if the connection is lost.
*/
void update(const franka::RobotState& robot_state);
......@@ -95,63 +105,61 @@ class FrankaHW : public hardware_interface::RobotHW {
/**
* Checks whether a requested controller can be run, based on the resources
* and
* interfaces it claims
* and interfaces it claims.
*
* @param[in] info Controllers to be running at the same time.
*
* @param[in] info A list of all controllers to be started, including the
* resources they claim
* @return Returns true in case of a conflict, false in case of valid
* controllers
* @return True in case of a conflict, false in case of valid controllers.
*/
bool checkForConflict(const std::list<hardware_interface::ControllerInfo>& info) const override;
/**
* Performs the switch between controllers and is real-time capable
*
* Performs controller switching (real-time capable).
*/
void doSwitch(const std::list<hardware_interface::ControllerInfo>&,
const std::list<hardware_interface::ControllerInfo>&) override;
/**
* Prepares the switching between controllers. This function is not real-time
* capable.
* Prepares switching between controllers (not real-time capable).
*
* @param[in] start_list Controllers requested to be started.
* @param[in] stop_list Controllers requested to be stopped.
*
* @param[in] start_list Information list about all controllers requested to be
* started
* @return True if the preparation has been successful, false otherwise.
*/
bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list) override;
/**
* Getter for the current Joint Position Command
* Gets the current joint position command.
*
* @return The current Joint Position command
* @return Current joint position command.
*/
std::array<double, 7> getJointPositionCommand() const;
std::array<double, 7> getJointPositionCommand() const noexcept;
/**
* Getter for the current Joint Velocity Command
* Gets the current joint velocity command.
*
* @return The current Joint Velocity command
* @return Current joint velocity command.
*/
std::array<double, 7> getJointVelocityCommand() const;
std::array<double, 7> getJointVelocityCommand() const noexcept;
/**
* Getter for the current Joint Torque Command
* Gets the current joint torque command.
*
* @return The current Joint Torque command
* @return Current joint torque command.
*/
std::array<double, 7> getJointEffortCommand() const;
std::array<double, 7> getJointEffortCommand() const noexcept;
/**
* Enforces joint limits on position velocity and torque level
* Enforces limits on position, velocity, and torque level.
*
* @param[in] kPeriod The duration of the current cycle
* @param[in] period Duration of the current cycle.
*/
void enforceLimits(const ros::Duration& period);
/**
* Resets the joint limits interface
* Resets the limit interfaces.
*/
void reset();
......
......@@ -9,34 +9,42 @@
namespace franka_hw {
/** A handle used to get the dynamic model of franka in joint-space. */
/**
* Handle to perform calculations using the dynamic model of a robot.
*/
class FrankaModelHandle {
public:
FrankaModelHandle() = delete;
/**
* Constructs an instance of a FrankaModelHandle
* Creates an instance of a FrankaModelHandle.
*
* @param[in] name The name of the handle
* @param[in] model A reference to a a franka::Model model instance
* @param[in] robot_state A reference to the storage of the current robot state
* @param[in] name The name of the model handle.
* @param[in] model A reference to the franka::Model instance wrapped by this handle.
* @param[in] robot_state A reference to the current robot state.
*/
FrankaModelHandle(const std::string& name, franka::Model& model, franka::RobotState& robot_state)
: name_(name), model_(&model), robot_state_(&robot_state) {}
/**
* Returns the resource name of the Handle
* Gets the name of the model handle.
*
* @return Name of the model handle.
*/
std::string getName() const { return name_; }
std::string getName() const noexcept { return name_; }
/**
* Returns the inertia matrix, given the current robot state and given external
* loads
* Calculates the 7x7 mass matrix from the current robot state. Unit: \f$[kg \times m^2]\f$.
*
* @param[in] load_inertia Inertia of the load, relative to center of mass, given as vectorized
* 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
* @param[in] load_mass Weight of the load. Unit: \f$[kg]\f$.
* @param[in] F_x_Cload Translation from flange to center of mass of load.
* Unit: \f$[m]\f$.
*
* @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
* frame
* @param[in] load_mass The mass of the load
* @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
* @return Vectorized 7x7 mass matrix, column-major.
*
* @see franka::Model::mass
*/
std::array<double, 49> getMass(
const std::array<double, 9>& load_inertia,
......@@ -46,14 +54,18 @@ class FrankaModelHandle {
}
/**
* Returns the inertia matrix, given the input robot state and given external
* loads
* Calculates the 7x7 mass matrix from a given robot state. Unit: \f$[kg \times m^2]\f$.
*
* @param[in] robot_state State from which the pose should be calculated.
* @param[in] load_inertia Inertia of the load, relative to center of mass, given as vectorized
* 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
* @param[in] load_mass Weight of the load. Unit: \f$[kg]\f$.
* @param[in] F_x_Cload Translation from flange to center of mass of load.
* Unit: \f$[m]\f$.
*
* @param[in] robot_state A user-given robot state to evaluate the dynamics
* @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
* frame
* @param[in] load_mass The mass of the load
* @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
* @return Vectorized 7x7 mass matrix, column-major.
*
* @see franka::Model::mass
*/
std::array<double, 49> getMass(
const franka::RobotState& robot_state,
......@@ -64,13 +76,18 @@ class FrankaModelHandle {
}
/**
* Returns the coriolis torques given the current robot state and given
* external loads
* Calculates the Coriolis force vector (state-space equation) from the current robot state:
* \f$ c= C \times dq\f$, in \f$[Nm]\f$.
*
* @param[in] load_inertia Inertia of the load, relative to center of mass, given as vectorized
* 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
* @param[in] load_mass Weight of the load. Unit: \f$[kg]\f$.
* @param[in] F_x_Cload Translation from flange to center of mass of load.
* Unit: \f$[m]\f$.
*
* @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
* frame
* @param[in] load_mass The mass of the load
* @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
* @return Coriolis force vector.
*
* @see franka::Model::coriolis
*/
std::array<double, 7> getCoriolis(
const std::array<double, 9>& load_inertia,
......@@ -80,14 +97,19 @@ class FrankaModelHandle {
}
/**
* Returns the coriolis torques, given the input robot state and given external
* loads
* Calculates the Coriolis force vector (state-space equation) from a given robot state:
* \f$ c= C \times dq\f$, in \f$[Nm]\f$.
*
* @param[in] robot_state State from which the Coriolis force vector should be calculated.
* @param[in] load_inertia Inertia of the load, relative to center of mass, given as vectorized
* 3x3 column-major matrix. Unit: \f$[kg \times m^2]\f$.
* @param[in] load_mass Weight of the load. Unit: \f$[kg]\f$.
* @param[in] F_x_Cload Translation from flange to center of mass of load.
* Unit: \f$[m]\f$.
*
* @param[in] robot_state A user-given robot state to evaluate the dynamics
* @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
* frame
* @param[in] load_mass The mass of the load
* @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
* @return Coriolis force vector.
*
* @see franka::Model::coriolis
*/
std::array<double, 7> getCoriolis(
const franka::RobotState& robot_state,
......@@ -98,13 +120,17 @@ class FrankaModelHandle {
}
/**
* Returns the gravity torques, given the current robot state and given
* external loads
* Calculates the gravity vector from the current robot state. Unit: \f$[Nm]\f$.
*
* @param[in] load_mass Weight of the load. Unit: \f$[kg]\f$.
* @param[in] F_x_Cload Translation from flange to center of mass of load.
* Unit: \f$[m]\f$.
* @param[in] gravity_earth Earth's gravity vector. Unit: \f$\frac{m}{s^2}\f$.
* Default to {0.0, 0.0, -9.81}.
*
* @return Gravity vector.
*
* @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
* frame
* @param[in] load_mass The mass of the load
* @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
* @see franka::Model::gravity
*/
std::array<double, 7> getGravity(
double load_mass,
......@@ -114,14 +140,18 @@ class FrankaModelHandle {
}
/**
* Returns the gravity torques, given the input robot state and given external
* loads
* Calculates the gravity vector from the given robot state. Unit: \f$[Nm]\f$.
*
* @param[in] robot_state A user-given robot state to evaluate the dynamics
* @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
* frame
* @param[in] load_mass The mass of the load
* @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
* @param[in] robot_state State from which the gravity vector should be calculated.
* @param[in] load_mass Weight of the load. Unit: \f$[kg]\f$.
* @param[in] F_x_Cload Translation from flange to center of mass of load.
* Unit: \f$[m]\f$.
* @param[in] gravity_earth Earth's gravity vector. Unit: \f$\frac{m}{s^2}\f$.
* Default to {0.0, 0.0, -9.81}.
*
* @return Gravity vector.
*
* @see franka::Model::gravity
*/
std::array<double, 7> getGravity(
const franka::RobotState& robot_state,
......@@ -132,79 +162,105 @@ class FrankaModelHandle {
}
/**
* Returns the Cartesian pose matrix of a frame w.r.t. the O-frame of the robot
* Gets the 4x4 pose matrix for the given frame in base frame, calculated from the current
* robot state.
*
* The pose is represented as a 4x4 matrix in column-major format.
*
* @param[in] frame The desired frame.
* @param[in] robot_state A user-given robot state to evaluate the forward kinematics.
*
* @return Vectorized 4x4 homogeneous transform, column-major.
* @return Vectorized 4x4 pose matrix, column-major.
*
* @see franka::Model::pose
*/
std::array<double, 16> getPose(const franka::Frame& frame,
const franka::RobotState& robot_state) {
return model_->pose(frame, robot_state);
std::array<double, 16> getPose(const franka::Frame& frame) const {
return getPose(frame, *robot_state_);
}
/**
* Returns the Cartesian pose matrix of a frame w.r.t. the O-frame of the robot
* Gets the 4x4 pose matrix for the given frame in base frame, calculated from the given
* robot state.
*
* The pose is represented as a 4x4 matrix in column-major format.
*
* @param[in] frame The desired frame.
* @param[in] robot_state State from which the pose should be calculated.
*
* @return Vectorized 4x4 pose matrix, column-major.
*
* @return Vectorized 4x4 homogeneous transform, column-major.
* @see franka::Model::pose
*/
std::array<double, 16> getPose(const franka::Frame& frame) {
return getPose(frame, *robot_state_);
std::array<double, 16> getPose(const franka::Frame& frame,
const franka::RobotState& robot_state) const {
return model_->pose(frame, robot_state);
}
/**
* Gets the 6x7 Jacobian for the given frame w.r.t. that frame. The Jacobian is represented
* as a 6x7 matrix in column-major format and is evaluated at the given robot state
* Gets the 6x7 Jacobian for the given frame, relative to that frame.
*
* The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
* the current robot state.
*
* @param[in] frame The desired frame.
* @param[in] robot_state State from which the pose should be calculated.
*
* @return Vectorized 6x7 Jacobian, column-major.
*
* @see franka::Model::bodyJacobian
*/
std::array<double, 42> getBodyJacobian(const franka::Frame& frame,
const franka::RobotState& robot_state) {
return model_->bodyJacobian(frame, robot_state);
std::array<double, 42> getBodyJacobian(const franka::Frame& frame) const {
return getBodyJacobian(frame, *robot_state_);
}
/**
* Gets the 6x7 Jacobian for the given frame w.r.t. that frame and for the current robot
* state. The Jacobian is represented as a 6x7 matrix in column-major format.
* Gets the 6x7 Jacobian for the given frame, relative to that frame.
*
* The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
* the given robot state.
*
* @param[in] frame The desired frame.
* @param[in] robot_state State from which the pose should be calculated.
*
* @return Vectorized 6x7 Jacobian, column-major.
*
* @see franka::Model::bodyJacobian
*/
std::array<double, 42> getBodyJacobian(const franka::Frame& frame) {
return getBodyJacobian(frame, *robot_state_);
std::array<double, 42> getBodyJacobian(const franka::Frame& frame,
const franka::RobotState& robot_state) const {
return model_->bodyJacobian(frame, robot_state);
}
/**
* Gets the 6x7 Jacobian for the given frame w.r.t. the link0 frame. The Jacobian is represented
* as a 6x7 matrix in column-major format and is evaluated at the given robot state
* Gets the 6x7 Jacobian for the given joint relative to the base frame.
*
* The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
* the current robot state.
*
* @param[in] frame The desired frame.
* @param[in] robot_state State from which the pose should be calculated.
*
* @return Vectorized 6x7 Jacobian, column-major.
*
* @see franka::Model::zeroJacobian
*/
std::array<double, 42> getZeroJacobian(const franka::Frame& frame,
const franka::RobotState& robot_state) {
return model_->zeroJacobian(frame, robot_state);
std::array<double, 42> getZeroJacobian(const franka::Frame& frame) const {
return getZeroJacobian(frame, *robot_state_);
}
/**
* Gets the 6x7 Jacobian for the given frame w.r.t. the link0 frame and for the current robot
* state. The Jacobian is represented as a 6x7 matrix in column-major format.
* Gets the 6x7 Jacobian for the given joint relative to the base frame.
*
* The Jacobian is represented as a 6x7 matrix in column-major format and calculated from
* the given robot state.
*
* @param[in] frame The desired frame.
* @param[in] robot_state State from which the pose should be calculated.
*
* @return Vectorized 6x7 Jacobian, column-major.
*
* @see franka::Model::zeroJacobian
*/
std::array<double, 42> getZeroJacobian(const franka::Frame& frame) {
return getZeroJacobian(frame, *robot_state_);
std::array<double, 42> getZeroJacobian(const franka::Frame& frame,
const franka::RobotState& robot_state) const {
return model_->zeroJacobian(frame, robot_state);
}
private:
......@@ -213,14 +269,8 @@ class FrankaModelHandle {
const franka::RobotState* robot_state_;
};
/** \brief Hardware interface to support reading the joint-space dynamics of a
* franka robot
*
* This \ref HardwareInterface supports reading the state of an array of named
* Franka joints, each of which has some position, velocity, and effort and
* additional quantities (e.g. collision and
* contact states etc.).
*
/**
* Hardware interface to perform calculations using the dynamic model of a robot.
*/
class FrankaModelInterface : public hardware_interface::HardwareResourceManager<FrankaModelHandle> {
};
......
#pragma once
#include <string>
#include <franka/robot_state.h>
#include <hardware_interface/internal/hardware_resource_manager.h>
#include <array>
#include <cassert>
#include <string>
namespace franka_hw {
/** A handle used to read the complete state of a franka robot. */
/**
* Handle to read the complete state of a robot.
*/
class FrankaStateHandle {
public:
FrankaStateHandle() = delete;
/**
* @param[in] name The name of the state handle
* @param[in] robot_state A reference to the robot_state
* Creates an instance of a FrankaStateHandle.
*
* @param[in] name The name of the state handle.
* @param[in] robot_state A reference to the robot state wrapped by this handle.
*/
FrankaStateHandle(const std::string& name, franka::RobotState& robot_state)
: name_(name), robot_state_(&robot_state) {}
const std::string& getName() const { return name_; }
const franka::RobotState& getRobotState() const { return *robot_state_; }
/**
* Gets the name of the state handle.
*
* @return Name of the state handle.
*/
const std::string& getName() const noexcept { return name_; }
/**
* Gets the current robot state.
*
* @return Current robot state.
*/
const franka::RobotState& getRobotState() const noexcept { return *robot_state_; }
private:
std::string name_;
const franka::RobotState* robot_state_;
};
/** \brief Hardware interface to support reading the complete robot state of a
* franka robot
/**
* Hardware interface to read the complete robot state.
*
* This HardwareInterface supports reading the state of a franka robot
* This inludes a collision state, a contact state, estimated external
* wrench exerted to the robot w.r.t. the end-effector frame and the
* robot base_link and the homogenous transformation from
* End-effector frame to base_link frame as well joint_specific states
* @see franka::RobotState for a description of the values included in the robot state.
*/
class FrankaStateInterface : public hardware_interface::HardwareResourceManager<FrankaStateHandle> {
};
......
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for FRANKA EMIKA research robots: https://frankaemika.github.io
*/
- builder: doxygen
javadoc_autobrief: YES
......@@ -338,15 +338,15 @@ bool FrankaHW::prepareSwitch(const std::list<hardware_interface::ControllerInfo>
return true;
}
std::array<double, 7> FrankaHW::getJointPositionCommand() const {
std::array<double, 7> FrankaHW::getJointPositionCommand() const noexcept {
return position_joint_command_.q;
}
std::array<double, 7> FrankaHW::getJointVelocityCommand() const {
std::array<double, 7> FrankaHW::getJointVelocityCommand() const noexcept {
return velocity_joint_command_.dq;
}
std::array<double, 7> FrankaHW::getJointEffortCommand() const {
std::array<double, 7> FrankaHW::getJointEffortCommand() const noexcept {
return effort_joint_command_.tau_J;
}
......
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for FRANKA EMIKA research robots: https://frankaemika.github.io
*/
- builder: doxygen
javadoc_autobrief: YES
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for FRANKA EMIKA research robots: https://frankaemika.github.io
*/
- builder: doxygen
javadoc_autobrief: YES
/**
* @mainpage
* @htmlinclude "manifest.html"
*
* Overview page for FRANKA EMIKA research robots: https://frankaemika.github.io
*/
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment