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

Trim franka_hw package.

parent bf5bc2aa
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 61 additions and 832 deletions
franka_hw/CMakeLists.txt
+ 5
− 100
View file @ 64cf5d63
......@@ -7,81 +7,26 @@ set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
find_package(catkin REQUIRED COMPONENTS
actionlib_msgs
controller_interface
controller_manager
franka_description
geometry_msgs
hardware_interface
joint_limits_interface
message_generation
pluginlib
realtime_tools
roscpp
sensor_msgs
std_msgs
tf
tf2_msgs
urdf
xmlrpcpp
)
find_package(Franka REQUIRED)
add_message_files(FILES
Errors.msg
FrankaState.msg
)
add_service_files(FILES
SetCartesianImpedance.srv
SetEEFrame.srv
SetForceTorqueCollisionBehavior.srv
SetFullCollisionBehavior.srv
SetJointImpedance.srv
SetKFrame.srv
SetLoad.srv
)
add_action_files(FILES
ErrorRecovery.action
)
generate_messages(DEPENDENCIES
actionlib_msgs
std_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES franka_hw franka_state_controller
CATKIN_DEPENDS actionlib_msgs controller_interface pluginlib
hardware_interface message_runtime realtime_tools roscpp
LIBRARIES franka_hw
CATKIN_DEPENDS controller_interface hardware_interface roscpp
DEPENDS Franka
)
add_library(franka_hw_services src/services.cpp)
add_dependencies(franka_hw_services
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(franka_hw_services
${catkin_LIBRARIES}
Franka::Franka
)
target_include_directories(franka_hw_services PUBLIC
${catkin_INCLUDE_DIRS}
include
)
add_library(franka_hw
src/control_mode.cpp
src/franka_hw.cpp
src/franka_controller_switching_types.cpp
src/franka_hw_helper_functions.cpp
src/resource_helpers.cpp
src/trigger_rate.cpp
)
......@@ -100,46 +45,6 @@ target_include_directories(franka_hw PUBLIC
include
)
add_library(franka_state_controller
src/franka_state_controller.cpp
)
add_dependencies(franka_state_controller
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
${PROJECT_NAME}_generate_messages_cpp
)
target_link_libraries(franka_state_controller
${catkin_LIBRARIES}
Franka::Franka
)
target_include_directories(franka_state_controller PUBLIC
${catkin_INCLUDE_DIRS}
include
)
add_executable(franka_hw_node
src/franka_hw_node.cpp
)
add_dependencies(franka_hw_node
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
franka_hw_services
)
target_link_libraries(franka_hw_node
${catkin_LIBRARIES}
franka_hw
franka_hw_services
)
target_include_directories(franka_hw_node PUBLIC
${catkin_INCLUDE_DIRS}
)
if(CATKIN_ENABLE_TESTING)
add_subdirectory(test)
endif()
......@@ -154,5 +59,5 @@ include(ClangTools)
add_format_target(franka_hw FILES ${SOURCES} ${HEADERS})
add_tidy_target(franka_hw
FILES ${SOURCES}
DEPENDS franka_hw franka_hw_node
DEPENDS franka_hw
)
franka_hw/action/ErrorRecovery.action deleted 100644 → 0
+ 0
− 2
View file @ bf5bc2aa
---
---
franka_hw/config/default_controllers.yaml deleted 100644 → 0
+ 0
− 66
View file @ bf5bc2aa
position_joint_trajectory_controller:
type: "position_controllers/JointTrajectoryController"
joints:
- franka_emika_joint1
- franka_emika_joint2
- franka_emika_joint3
- franka_emika_joint4
- franka_emika_joint5
- franka_emika_joint6
- franka_emika_joint7
constraints:
goal_time: 0.5
franka_emika_joint1:
goal: 0.05
franka_emika_joint2:
goal: 0.05
franka_emika_joint3:
goal: 0.05
franka_emika_joint4:
goal: 0.05
franka_emika_joint5:
goal: 0.05
franka_emika_joint6:
goal: 0.05
franka_emika_joint7:
goal: 0.05
effort_joint_trajectory_controller:
type: "effort_controllers/JointTrajectoryController"
joints:
- franka_emika_joint1
- franka_emika_joint2
- franka_emika_joint3
- franka_emika_joint4
- franka_emika_joint5
- franka_emika_joint6
- franka_emika_joint7
constraints:
goal_time: 0.5
franka_emika_joint1:
goal: 0.05
franka_emika_joint2:
goal: 0.05
franka_emika_joint3:
goal: 0.05
franka_emika_joint4:
goal: 0.05
franka_emika_joint5:
goal: 0.05
franka_emika_joint6:
goal: 0.05
franka_emika_joint7:
goal: 0.05
gains:
franka_emika_joint1: {p: 100, d: 10, i: 0, i_clamp: 1}
franka_emika_joint2: {p: 100, d: 10, i: 0, i_clamp: 1}
franka_emika_joint3: {p: 100, d: 10, i: 0, i_clamp: 1}
franka_emika_joint4: {p: 100, d: 10, i: 0, i_clamp: 1}
franka_emika_joint5: {p: 100, d: 10, i: 0, i_clamp: 1}
franka_emika_joint6: {p: 100, d: 10, i: 0, i_clamp: 1}
franka_emika_joint7: {p: 100, d: 10, i: 0, i_clamp: 1}
franka_state_controller:
type: franka_hw/FrankaStateController
publish_rate: 30 # [Hz]
franka_hw/config/franka_hw.yaml deleted 100644 → 0
+ 0
− 11
View file @ bf5bc2aa
joint_names:
- franka_emika_joint1
- franka_emika_joint2
- franka_emika_joint3
- franka_emika_joint4
- franka_emika_joint5
- franka_emika_joint6
- franka_emika_joint7
arm_id: franka_emika
robot_ip: 127.0.0.1
franka_hw/franka_hw_controller_plugin.xml deleted 100644 → 0
+ 0
− 7
View file @ bf5bc2aa
<library path="lib/libfranka_state_controller">
<class name="franka_hw/FrankaStateController" type="franka_hw::FrankaStateController" base_class_type="controller_interface::ControllerBase">
<description>
A controller that publishes all relevant franka robot states
</description>
</class>
</library>
franka_hw/include/franka_hw/franka_controller_switching_types.h → franka_hw/include/franka_hw/control_mode.h
+ 0
− 16
View file @ 64cf5d63
#pragma once
#include <cstdint>
#include <map>
#include <ostream>
#include <string>
#include <vector>
namespace franka_hw {
using ResourceWithClaimsMap = std::map<std::string, std::vector<std::vector<std::string>>>;
struct ResourceClaims {
uint8_t joint_position_claims = 0;
uint8_t joint_velocity_claims = 0;
uint8_t joint_torque_claims = 0;
uint8_t cartesian_velocity_claims = 0;
uint8_t cartesian_pose_claims = 0;
};
using ArmClaimedMap = std::map<std::string, ResourceClaims>;
enum class ControlMode {
None = 0,
JointTorque = (1 << 0),
......
franka_hw/include/franka_hw/franka_hw.h
+ 1
− 1
View file @ 64cf5d63
......@@ -16,8 +16,8 @@
#include <ros/node_handle.h>
#include <ros/time.h>
#include <franka_hw/control_mode.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <franka_hw/franka_controller_switching_types.h>
#include <franka_hw/franka_model_interface.h>
#include <franka_hw/franka_state_interface.h>
......
franka_hw/include/franka_hw/franka_state_controller.h deleted 100644 → 0
+ 0
− 91
View file @ bf5bc2aa
#pragma once
#include <cstdint>
#include <memory>
#include <vector>
#include <controller_interface/multi_interface_controller.h>
#include <franka_hw/FrankaState.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <hardware_interface/robot_hw.h>
#include <realtime_tools/realtime_publisher.h>
#include <sensor_msgs/JointState.h>
#include <tf2_msgs/TFMessage.h>
#include <franka_hw/FrankaState.h>
#include <franka_hw/franka_state_interface.h>
#include <franka_hw/trigger_rate.h>
namespace franka_hw {
class FrankaStateController
: public controller_interface::MultiInterfaceController<franka_hw::FrankaStateInterface> {
public:
FrankaStateController();
/**
* 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
*/
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
*
* @param[in] time Current ros time
*/
void update(const ros::Time& time, const ros::Duration&) 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_;
franka_hw::FrankaStateInterface* franka_state_interface_;
std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> publisher_transforms_;
realtime_tools::RealtimePublisher<franka_hw::FrankaState> publisher_franka_states_;
realtime_tools::RealtimePublisher<sensor_msgs::JointState> publisher_joint_states_;
realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> publisher_external_wrench_;
franka_hw::TriggerRate trigger_publish_;
franka::RobotState robot_state_;
uint64_t sequence_number_ = 0;
std::vector<std::string> joint_names_;
};
} // namespace franka_hw
franka_hw/include/franka_hw/services.h deleted 100644 → 0
+ 0
− 57
View file @ bf5bc2aa
#pragma once
#include <franka/exception.h>
#include <franka/robot.h>
#include <ros/console.h>
#include <ros/node_handle.h>
#include <ros/service_server.h>
#include <franka_hw/SetCartesianImpedance.h>
#include <franka_hw/SetEEFrame.h>
#include <franka_hw/SetForceTorqueCollisionBehavior.h>
#include <franka_hw/SetFullCollisionBehavior.h>
#include <franka_hw/SetJointImpedance.h>
#include <franka_hw/SetKFrame.h>
#include <franka_hw/SetLoad.h>
namespace franka_hw {
namespace services {
template <typename T>
ros::ServiceServer advertiseService(
ros::NodeHandle& node_handle,
const std::string& name,
std::function<void(typename T::Request&, typename T::Response&)> handler) {
return node_handle.advertiseService<typename T::Request, typename T::Response>(
name, [name, handler](typename T::Request& request, typename T::Response& response) {
try {
handler(request, response);
response.success = true;
ROS_DEBUG_STREAM(name << " succeeded.");
} catch (const franka::Exception& ex) {
ROS_ERROR_STREAM(name << " failed: " << ex.what());
response.success = false;
response.error = ex.what();
}
return true;
});
}
void setCartesianImpedance(franka::Robot& robot,
const SetCartesianImpedance::Request& req,
SetCartesianImpedance::Response& res);
void setJointImpedance(franka::Robot& robot,
const SetJointImpedance::Request& req,
SetJointImpedance::Response& res);
void setEEFrame(franka::Robot& robot, const SetEEFrame::Request& req, SetEEFrame::Response& res);
void setKFrame(franka::Robot& robot, const SetKFrame::Request& req, SetKFrame::Response& res);
void setForceTorqueCollisionBehavior(franka::Robot& robot,
const SetForceTorqueCollisionBehavior::Request& req,
SetForceTorqueCollisionBehavior::Response& res);
void setFullCollisionBehavior(franka::Robot& robot,
const SetFullCollisionBehavior::Request& req,
SetFullCollisionBehavior::Response& res);
void setLoad(franka::Robot& robot, const SetLoad::Request& req, SetLoad::Response& res);
} // namespace services
} // namespace franka_hw
franka_hw/launch/franka_hw.launch deleted 100644 → 0
+ 0
− 26
View file @ bf5bc2aa
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" default="franka_emika" />
<arg name="load_gripper" default="true" />
<node name="$(arg arm_id)" pkg="franka_hw" type="franka_hw_node" output="screen" required="true">
<rosparam command="load" file="$(find franka_hw)/config/franka_hw.yaml" />
<param name="robot_ip" value="$(arg robot_ip)" />
<param name="arm_id" value="$(arg arm_id)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01_Gripper.urdf.xacro'" if="$(arg load_gripper)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01.urdf.xacro'" unless="$(arg load_gripper)" />
</node>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_hw)/config/default_controllers.yaml" />
<node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller --shutdown-timeout 2"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/FEA_01_01_Gripper.urdf.xacro'" />
<rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper_node/joint_states] </rosparam>
<rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
<param name="rate" value="30"/>
</node>
</group>
</launch>
franka_hw/msg/Errors.msg deleted 100644 → 0
+ 0
− 33
View file @ bf5bc2aa
bool joint_position_limits_violation
bool cartesian_position_limits_violation
bool self_collision_avoidance_violation
bool joint_velocity_violation
bool cartesian_velocity_violation
bool force_control_safety_violation
bool joint_reflex
bool cartesian_reflex
bool max_goal_pose_deviation_violation
bool max_path_pose_deviation_violation
bool cartesian_velocity_profile_safety_violation
bool joint_position_motion_generator_start_pose_invalid
bool joint_motion_generator_position_limits_violation
bool joint_motion_generator_velocity_limits_violation
bool joint_motion_generator_velocity_discontinuity
bool joint_motion_generator_acceleration_discontinuity
bool cartesian_position_motion_generator_start_pose_invalid
bool cartesian_motion_generator_elbow_limit_violation
bool cartesian_motion_generator_velocity_limits_violation
bool cartesian_motion_generator_velocity_discontinuity
bool cartesian_motion_generator_acceleration_discontinuity
bool cartesian_motion_generator_elbow_sign_inconsistent
bool cartesian_motion_generator_start_elbow_invalid
bool cartesian_motion_generator_joint_position_limits_violation
bool cartesian_motion_generator_joint_velocity_limits_violation
bool cartesian_motion_generator_joint_velocity_discontinuity
bool cartesian_motion_generator_joint_acceleration_discontinuity
bool cartesian_position_motion_generator_invalid_frame
bool force_controller_desired_force_tolerance_violation
bool controller_torque_discontinuity
bool start_elbow_sign_inconsistent
bool communication_constraints_violation
bool power_limit_violation
franka_hw/msg/FrankaState.msg deleted 100644 → 0
+ 0
− 26
View file @ bf5bc2aa
std_msgs/Header header
float64[6] cartesian_collision
float64[6] cartesian_contact
float64[7] q
float64[7] q_d
float64[7] dq
float64[7] dq_d
float64[7] tau_J
float64[7] dtau_J
float64[6] K_F_ext_hat_K
float64[2] elbow
float64[2] elbow_d
float64[7] joint_collision
float64[7] joint_contact
float64[6] O_F_ext_hat_K
float64[7] tau_ext_hat_filtered
float64 m_load
float64[3] F_x_Cload
float64[9] I_load
float64[16] O_T_EE
float64[16] O_T_EE_d
float64[16] F_T_EE
float64[16] EE_T_K
float64 time
franka_hw/Errors current_errors
franka_hw/Errors last_motion_errors
franka_hw/package.xml
+ 2
− 22
View file @ 64cf5d63
......@@ -2,7 +2,7 @@
<package format="2">
<name>franka_hw</name>
<version>0.0.1</version>
<description>This package implements the franka_hw of Franka Emika for the use with ros_control</description>
<description>franka_hw provides hardware interfaces for using FRANKA EMIKA research robots with ros_control</description>
<maintainer email="info@franka.de">FRANKA EMIKA Research Interface Team</maintainer>
<license>NONE</license>
......@@ -13,32 +13,12 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<depend>Franka</depend>
<depend>actionlib</depend>
<depend>actionlib_msgs</depend>
<depend>controller_interface</depend>
<depend>controller_manager</depend>
<depend>geometry_msgs</depend>
<depend>hardware_interface</depend>
<depend>joint_limits_interface</depend>
<depend>pluginlib</depend>
<depend>realtime_tools</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2_msgs</depend>
<depend>tf</depend>
<depend>urdf</depend>
<depend>xmlrpcpp</depend>
<exec_depend>message_runtime</exec_depend>
<test_depend>gtest</test_depend>
<test_depend>rostest</test_depend>
<export>
<controller_interface plugin="${prefix}/franka_hw_controller_plugin.xml"/>
</export>
</package>
franka_hw/src/control_mode.cpp 0 → 100644
+ 34
− 0
View file @ 64cf5d63
#include <franka_hw/control_mode.h>
#include <algorithm>
#include <iterator>
namespace franka_hw {
std::ostream& operator<<(std::ostream& ostream, ControlMode mode) {
if (mode == ControlMode::None) {
ostream << "<none>";
} else {
std::vector<std::string> names;
if ((mode & ControlMode::JointTorque) != ControlMode::None) {
names.emplace_back("JointTorque");
}
if ((mode & ControlMode::JointPosition) != ControlMode::None) {
names.emplace_back("JointPosition");
}
if ((mode & ControlMode::JointVelocity) != ControlMode::None) {
names.emplace_back("JointVelocity");
}
if ((mode & ControlMode::CartesianVelocity) != ControlMode::None) {
names.emplace_back("CartesianVelocity");
}
if ((mode & ControlMode::CartesianPose) != ControlMode::None) {
names.emplace_back("CartesianPose");
}
std::copy(names.cbegin(), names.cend() - 1, std::ostream_iterator<std::string>(ostream, ", "));
ostream << names.back();
}
return ostream;
}
} // namespace franka_hw
franka_hw/src/franka_controller_switching_types.cpp
+ 1
− 1
View file @ 64cf5d63
#include <franka_hw/franka_controller_switching_types.h>
#include "franka_hw/control_mode.h"
#include <algorithm>
#include <iterator>
......
franka_hw/src/franka_hw.cpp
+ 1
− 1
View file @ 64cf5d63
......@@ -5,7 +5,7 @@
#include <joint_limits_interface/joint_limits_urdf.h>
#include <urdf/model.h>
#include "franka_hw_helper_functions.h"
#include "resource_helpers.h"
namespace franka_hw {
......
franka_hw/src/franka_state_controller.cpp deleted 100644 → 0
+ 0
− 365
View file @ bf5bc2aa
#include <franka_hw/franka_state_controller.h>
#include <cmath>
#include <mutex>
#include <string>
#include <controller_interface/controller_base.h>
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include <franka_hw/Errors.h>
#include <franka_hw/franka_cartesian_command_interface.h>
namespace {
tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
tf::Matrix3x3 rotation(transform[0], transform[4], transform[8], transform[1], transform[5],
transform[9], transform[2], transform[6], transform[10]);
tf::Vector3 translation(transform[12], transform[13], transform[14]);
return tf::Transform(rotation, translation);
}
franka_hw::Errors errorsToMessage(const franka::Errors& error) {
franka_hw::Errors message;
message.joint_position_limits_violation =
static_cast<decltype(message.joint_position_limits_violation)>(
error.joint_position_limits_violation);
message.cartesian_position_limits_violation =
static_cast<decltype(message.cartesian_position_limits_violation)>(
error.cartesian_position_limits_violation);
message.self_collision_avoidance_violation =
static_cast<decltype(message.self_collision_avoidance_violation)>(
error.self_collision_avoidance_violation);
message.joint_velocity_violation =
static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
message.cartesian_velocity_violation =
static_cast<decltype(message.cartesian_velocity_violation)>(
error.cartesian_velocity_violation);
message.force_control_safety_violation =
static_cast<decltype(message.force_control_safety_violation)>(
error.force_control_safety_violation);
message.joint_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
message.cartesian_reflex =
static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
message.max_goal_pose_deviation_violation =
static_cast<decltype(message.max_goal_pose_deviation_violation)>(
error.max_goal_pose_deviation_violation);
message.max_path_pose_deviation_violation =
static_cast<decltype(message.max_path_pose_deviation_violation)>(
error.max_path_pose_deviation_violation);
message.cartesian_velocity_profile_safety_violation =
static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
error.cartesian_velocity_profile_safety_violation);
message.joint_position_motion_generator_start_pose_invalid =
static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
error.joint_position_motion_generator_start_pose_invalid);
message.joint_motion_generator_position_limits_violation =
static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
error.joint_motion_generator_position_limits_violation);
message.joint_motion_generator_velocity_limits_violation =
static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
error.joint_motion_generator_velocity_limits_violation);
message.joint_motion_generator_velocity_discontinuity =
static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
error.joint_motion_generator_velocity_discontinuity);
message.joint_motion_generator_acceleration_discontinuity =
static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
error.joint_motion_generator_acceleration_discontinuity);
message.cartesian_position_motion_generator_start_pose_invalid =
static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
error.cartesian_position_motion_generator_start_pose_invalid);
message.cartesian_motion_generator_elbow_limit_violation =
static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
error.cartesian_motion_generator_elbow_limit_violation);
message.cartesian_motion_generator_velocity_limits_violation =
static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
error.cartesian_motion_generator_velocity_limits_violation);
message.cartesian_motion_generator_velocity_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
error.cartesian_motion_generator_velocity_discontinuity);
message.cartesian_motion_generator_acceleration_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
error.cartesian_motion_generator_acceleration_discontinuity);
message.cartesian_motion_generator_elbow_sign_inconsistent =
static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
error.cartesian_motion_generator_elbow_sign_inconsistent);
message.cartesian_motion_generator_start_elbow_invalid =
static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
error.cartesian_motion_generator_start_elbow_invalid);
message.cartesian_motion_generator_joint_position_limits_violation =
static_cast<decltype(message.cartesian_motion_generator_joint_position_limits_violation)>(
error.cartesian_motion_generator_joint_position_limits_violation);
message.cartesian_motion_generator_joint_velocity_limits_violation =
static_cast<decltype(message.cartesian_motion_generator_joint_velocity_limits_violation)>(
error.cartesian_motion_generator_joint_velocity_limits_violation);
message.cartesian_motion_generator_joint_velocity_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_joint_velocity_discontinuity)>(
error.cartesian_motion_generator_joint_velocity_discontinuity);
message.cartesian_motion_generator_joint_acceleration_discontinuity =
static_cast<decltype(message.cartesian_motion_generator_joint_acceleration_discontinuity)>(
error.cartesian_motion_generator_joint_acceleration_discontinuity);
message.cartesian_position_motion_generator_invalid_frame =
static_cast<decltype(message.cartesian_position_motion_generator_invalid_frame)>(
error.cartesian_position_motion_generator_invalid_frame);
message.force_controller_desired_force_tolerance_violation =
static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
error.force_controller_desired_force_tolerance_violation);
message.controller_torque_discontinuity =
static_cast<decltype(message.controller_torque_discontinuity)>(
error.controller_torque_discontinuity);
message.start_elbow_sign_inconsistent =
static_cast<decltype(message.start_elbow_sign_inconsistent)>(
error.start_elbow_sign_inconsistent);
message.communication_constraints_violation =
static_cast<decltype(message.communication_constraints_violation)>(
error.communication_constraints_violation);
message.power_limit_violation =
static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
return message;
}
} // anonymous namespace
namespace franka_hw {
FrankaStateController::FrankaStateController()
: franka_state_interface_(nullptr),
franka_state_handle_(nullptr),
publisher_transforms_(),
publisher_franka_states_(),
publisher_joint_states_(),
publisher_external_wrench_(),
trigger_publish_(30.0) {}
bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& controller_node_handle) {
franka_state_interface_ = robot_hardware->get<franka_hw::FrankaStateInterface>();
if (franka_state_interface_ == nullptr) {
ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
return false;
}
if (!root_node_handle.getParam("arm_id", arm_id_)) {
ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
return false;
}
double publish_rate(30.0);
if (controller_node_handle.getParam("publish_rate", publish_rate)) {
trigger_publish_ = franka_hw::TriggerRate(publish_rate);
} else {
ROS_INFO_STREAM("FrankaStateController: Did not find publish_rate. Using default "
<< publish_rate << " [Hz].");
}
if (!root_node_handle.getParam("joint_names", joint_names_) ||
joint_names_.size() != robot_state_.q.size()) {
ROS_ERROR(
"FrankaStateController: Invalid or no joint_names parameters provided, aborting "
"controller init!");
return false;
}
try {
franka_state_handle_.reset(
new franka_hw::FrankaStateHandle(franka_state_interface_->getHandle(arm_id_ + "_robot")));
} catch (const hardware_interface::HardwareInterfaceException& ex) {
ROS_ERROR_STREAM("FrankaStateController: Exception getting cartesian handle: " << ex.what());
return false;
}
publisher_transforms_.init(root_node_handle, "/tf", 1);
publisher_franka_states_.init(controller_node_handle, "franka_states", 1);
publisher_joint_states_.init(controller_node_handle, "joint_states", 1);
publisher_external_wrench_.init(controller_node_handle, "F_ext", 1);
{
std::lock_guard<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > lock(
publisher_joint_states_);
publisher_joint_states_.msg_.name.resize(joint_names_.size());
publisher_joint_states_.msg_.position.resize(robot_state_.q.size());
publisher_joint_states_.msg_.velocity.resize(robot_state_.dq.size());
publisher_joint_states_.msg_.effort.resize(robot_state_.tau_J.size());
}
{
std::lock_guard<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage> > lock(
publisher_transforms_);
publisher_transforms_.msg_.transforms.resize(2);
tf::Quaternion quaternion(0.0, 0.0, 0.0, 1.0);
tf::Vector3 translation(0.0, 0.0, 0.05);
tf::Transform transform(quaternion, translation);
tf::StampedTransform stamped_transform(transform, ros::Time::now(), arm_id_ + "_link8",
arm_id_ + "_EE");
geometry_msgs::TransformStamped transform_message;
transformStampedTFToMsg(stamped_transform, transform_message);
publisher_transforms_.msg_.transforms[0] = transform_message;
translation = tf::Vector3(0.0, 0.0, 0.0);
transform = tf::Transform(quaternion, translation);
stamped_transform =
tf::StampedTransform(transform, ros::Time::now(), arm_id_ + "_EE", arm_id_ + "_K");
transformStampedTFToMsg(stamped_transform, transform_message);
publisher_transforms_.msg_.transforms[1] = transform_message;
}
{
std::lock_guard<realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped> > lock(
publisher_external_wrench_);
publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
publisher_external_wrench_.msg_.wrench.force.x = 0.0;
publisher_external_wrench_.msg_.wrench.force.y = 0.0;
publisher_external_wrench_.msg_.wrench.force.z = 0.0;
publisher_external_wrench_.msg_.wrench.torque.x = 0.0;
publisher_external_wrench_.msg_.wrench.torque.y = 0.0;
publisher_external_wrench_.msg_.wrench.torque.z = 0.0;
}
return true;
}
void FrankaStateController::update(const ros::Time& time, const ros::Duration& /*period*/) {
if (trigger_publish_()) {
robot_state_ = franka_state_handle_->getRobotState();
publishFrankaStates(time);
publishTransforms(time);
publishExternalWrench(time);
publishJointStates(time);
sequence_number_++;
}
}
void FrankaStateController::publishFrankaStates(const ros::Time& time) {
if (publisher_franka_states_.trylock()) {
static_assert(
sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.cartesian_contact),
"Robot state Cartesian members do not have same size");
static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.K_F_ext_hat_K),
"Robot state Cartesian members do not have same size");
static_assert(sizeof(robot_state_.cartesian_collision) == sizeof(robot_state_.O_F_ext_hat_K),
"Robot state Cartesian members do not have same size");
for (size_t i = 0; i < robot_state_.cartesian_collision.size(); i++) {
publisher_franka_states_.msg_.cartesian_collision[i] = robot_state_.cartesian_collision[i];
publisher_franka_states_.msg_.cartesian_contact[i] = robot_state_.cartesian_contact[i];
publisher_franka_states_.msg_.K_F_ext_hat_K[i] = robot_state_.K_F_ext_hat_K[i];
publisher_franka_states_.msg_.O_F_ext_hat_K[i] = robot_state_.O_F_ext_hat_K[i];
}
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.q_d),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq_d),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_ext_hat_filtered),
"Robot state joint members do not have same size");
for (size_t i = 0; i < robot_state_.q.size(); i++) {
publisher_franka_states_.msg_.q[i] = robot_state_.q[i];
publisher_franka_states_.msg_.q_d[i] = robot_state_.q_d[i];
publisher_franka_states_.msg_.dq[i] = robot_state_.dq[i];
publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i];
publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i];
publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i];
publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i];
publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i];
publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i];
}
for (size_t i = 0; i < robot_state_.elbow.size(); i++) {
publisher_franka_states_.msg_.elbow[i] = robot_state_.elbow[i];
}
for (size_t i = 0; i < robot_state_.elbow_d.size(); i++) {
publisher_franka_states_.msg_.elbow_d[i] = robot_state_.elbow_d[i];
}
static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.F_T_EE),
"Robot state transforms do not have same size");
static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.EE_T_K),
"Robot state transforms do not have same size");
static_assert(sizeof(robot_state_.O_T_EE) == sizeof(robot_state_.O_T_EE_d),
"Robot state transforms do not have same size");
for (size_t i = 0; i < robot_state_.O_T_EE.size(); i++) {
publisher_franka_states_.msg_.O_T_EE[i] = robot_state_.O_T_EE[i];
publisher_franka_states_.msg_.F_T_EE[i] = robot_state_.F_T_EE[i];
publisher_franka_states_.msg_.EE_T_K[i] = robot_state_.EE_T_K[i];
publisher_franka_states_.msg_.O_T_EE_d[i] = robot_state_.O_T_EE_d[i];
}
publisher_franka_states_.msg_.m_load = robot_state_.m_load;
for (size_t i = 0; i < robot_state_.I_load.size(); i++) {
publisher_franka_states_.msg_.I_load[i] = robot_state_.I_load[i];
}
for (size_t i = 0; i < robot_state_.F_x_Cload.size(); i++) {
publisher_franka_states_.msg_.F_x_Cload[i] = robot_state_.F_x_Cload[i];
}
publisher_franka_states_.msg_.time = robot_state_.time.toSec();
publisher_franka_states_.msg_.current_errors = errorsToMessage(robot_state_.current_errors);
publisher_franka_states_.msg_.last_motion_errors =
errorsToMessage(robot_state_.last_motion_errors);
publisher_franka_states_.msg_.header.seq = sequence_number_;
publisher_franka_states_.msg_.header.stamp = time;
publisher_franka_states_.unlockAndPublish();
}
}
void FrankaStateController::publishJointStates(const ros::Time& time) {
if (publisher_joint_states_.trylock()) {
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dq),
"Robot state joint members do not have same size");
static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J),
"Robot state joint members do not have same size");
for (size_t i = 0; i < robot_state_.q.size(); i++) {
publisher_joint_states_.msg_.name[i] = joint_names_[i];
publisher_joint_states_.msg_.position[i] = robot_state_.q[i];
publisher_joint_states_.msg_.velocity[i] = robot_state_.dq[i];
publisher_joint_states_.msg_.effort[i] = robot_state_.tau_J[i];
}
publisher_joint_states_.msg_.header.stamp = time;
publisher_joint_states_.msg_.header.seq = sequence_number_;
publisher_joint_states_.unlockAndPublish();
}
}
void FrankaStateController::publishTransforms(const ros::Time& time) {
if (publisher_transforms_.trylock()) {
tf::StampedTransform stamped_transform(convertArrayToTf(robot_state_.F_T_EE), time,
arm_id_ + "_link8", arm_id_ + "_EE");
geometry_msgs::TransformStamped transform_message;
transformStampedTFToMsg(stamped_transform, transform_message);
publisher_transforms_.msg_.transforms[0] = transform_message;
stamped_transform = tf::StampedTransform(convertArrayToTf(robot_state_.EE_T_K), time,
arm_id_ + "_EE", arm_id_ + "_K");
transformStampedTFToMsg(stamped_transform, transform_message);
publisher_transforms_.msg_.transforms[1] = transform_message;
publisher_transforms_.unlockAndPublish();
}
}
void FrankaStateController::publishExternalWrench(const ros::Time& time) {
if (publisher_external_wrench_.trylock()) {
publisher_external_wrench_.msg_.header.frame_id = arm_id_ + "_K";
publisher_external_wrench_.msg_.header.stamp = time;
publisher_external_wrench_.msg_.wrench.force.x = robot_state_.K_F_ext_hat_K[0];
publisher_external_wrench_.msg_.wrench.force.y = robot_state_.K_F_ext_hat_K[1];
publisher_external_wrench_.msg_.wrench.force.z = robot_state_.K_F_ext_hat_K[2];
publisher_external_wrench_.msg_.wrench.torque.x = robot_state_.K_F_ext_hat_K[3];
publisher_external_wrench_.msg_.wrench.torque.y = robot_state_.K_F_ext_hat_K[4];
publisher_external_wrench_.msg_.wrench.torque.z = robot_state_.K_F_ext_hat_K[5];
publisher_external_wrench_.unlockAndPublish();
}
}
} // namespace franka_hw
PLUGINLIB_EXPORT_CLASS(franka_hw::FrankaStateController, controller_interface::ControllerBase)
franka_hw/src/franka_hw_helper_functions.cpp → franka_hw/src/resource_helpers.cpp
+ 1
− 1
View file @ 64cf5d63
#include "franka_hw_helper_functions.h"
#include "resource_helpers.h"
#include <ros/console.h>
......
franka_hw/src/franka_hw_helper_functions.h → franka_hw/src/resource_helpers.h
+ 16
− 1
View file @ 64cf5d63
#pragma once
#include <list>
#include <map>
#include <string>
#include <vector>
#include <franka_hw/franka_controller_switching_types.h>
#include <hardware_interface/controller_info.h>
#include <franka_hw/control_mode.h>
namespace franka_hw {
using ResourceWithClaimsMap = std::map<std::string, std::vector<std::vector<std::string>>>;
struct ResourceClaims {
uint8_t joint_position_claims = 0;
uint8_t joint_velocity_claims = 0;
uint8_t joint_torque_claims = 0;
uint8_t cartesian_velocity_claims = 0;
uint8_t cartesian_pose_claims = 0;
};
using ArmClaimedMap = std::map<std::string, ResourceClaims>;
bool findArmIdInResourceId(const std::string& resource_id, std::string* arm_id);
ResourceWithClaimsMap getResourceMap(const std::list<hardware_interface::ControllerInfo>& info);
......
franka_hw/srv/SetCartesianImpedance.srv deleted 100644 → 0
+ 0
− 5
View file @ bf5bc2aa
float64[6] cartesian_stiffness
---
bool success
string error
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment