21 "ForceExampleController: Make sure your robot's endeffector is in contact " 22 "with a horizontal surface before starting the controller!");
23 if (!node_handle.
getParam(
"arm_id", arm_id)) {
24 ROS_ERROR(
"ForceExampleController: Could not read parameter arm_id");
27 if (!node_handle.
getParam(
"joint_names", joint_names) || joint_names.size() != 7) {
29 "ForceExampleController: Invalid or no joint_names parameters provided, aborting " 35 if (model_interface ==
nullptr) {
36 ROS_ERROR_STREAM(
"ForceExampleController: Error getting model interface from hardware");
40 model_handle_ = std::make_unique<franka_hw::FrankaModelHandle>(
41 model_interface->getHandle(arm_id +
"_model"));
44 "ForceExampleController: Exception getting model handle from interface: " << ex.
what());
49 if (state_interface ==
nullptr) {
50 ROS_ERROR_STREAM(
"ForceExampleController: Error getting state interface from hardware");
54 state_handle_ = std::make_unique<franka_hw::FrankaStateHandle>(
55 state_interface->getHandle(arm_id +
"_robot"));
58 "ForceExampleController: Exception getting state handle from interface: " << ex.
what());
63 if (effort_joint_interface ==
nullptr) {
64 ROS_ERROR_STREAM(
"ForceExampleController: Error getting effort joint interface from hardware");
67 for (
size_t i = 0; i < 7; ++i) {
69 joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
79 dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>(
90 std::array<double, 7> gravity_array =
model_handle_->getGravity();
91 Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.
tau_J.data());
92 Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
100 std::array<double, 42> jacobian_array =
102 std::array<double, 7> gravity_array =
model_handle_->getGravity();
103 Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
104 Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.
tau_J.data());
105 Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d(
107 Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
109 Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
110 desired_force_torque.setZero();
113 tau_d << jacobian.transpose() * desired_force_torque;
119 for (
size_t i = 0; i < 7; ++i) {
130 franka_example_controllers::desired_mass_paramConfig& config,
138 const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
139 const Eigen::Matrix<double, 7, 1>& tau_J_d) {
140 Eigen::Matrix<double, 7, 1> tau_d_saturated{};
141 for (
size_t i = 0; i < 7; i++) {
142 double difference = tau_d_calculated[i] - tau_J_d[i];
145 return tau_d_saturated;
virtual const char * what() const
bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &node_handle) override
Eigen::Matrix< double, 7, 1 > saturateTorqueRate(const Eigen::Matrix< double, 7, 1 > &tau_d_calculated, const Eigen::Matrix< double, 7, 1 > &tau_J_d)
std::array< std::string, 7 > joint_names
void desiredMassParamCallback(franka_example_controllers::desired_mass_paramConfig &config, uint32_t level)
std::vector< hardware_interface::JointHandle > joint_handles_
std::unique_ptr< dynamic_reconfigure::Server< franka_example_controllers::desired_mass_paramConfig > > dynamic_server_desired_mass_param_
std::unique_ptr< franka_hw::FrankaModelHandle > model_handle_
static constexpr double kDeltaTauMax
void starting(const ros::Time &) override
std::array< double, 7 > tau_J_d
bool getParam(const std::string &key, std::string &s) const
std::unique_ptr< franka_hw::FrankaStateHandle > state_handle_
PLUGINLIB_EXPORT_CLASS(moveit_fake_controller_manager::MoveItFakeControllerManager, moveit_controller_manager::MoveItControllerManager)
ros::NodeHandle dynamic_reconfigure_desired_mass_param_node_
#define ROS_ERROR_STREAM(args)
std::array< double, 7 > tau_J
Eigen::Matrix< double, 7, 1 > tau_error_
Eigen::Matrix< double, 7, 1 > tau_ext_initial_
void update(const ros::Time &, const ros::Duration &period) override