Skip to content

Commit c335b6b

Browse files
BarisYaziciBarisYazici
committed
Merge pull request frankaemika#46 in SRR/franka_ros2 from feat/joint-position-interface to humble
* commit '9868cde2d966e8d3dbd86729382ec12d93de1a96': bump version 0.1.7 chore: update joint impedance example controller time chore: fix linting errors chore: devcontainer packages update hotfix: automatic acknowledge for reflex errors hotfix: current state during the control assigned correctly feat: joint position command interface
2 parents e4b7a90 + 9868cde commit c335b6b

31 files changed

+583
-92
lines changed

‎.devcontainer/Dockerfile

+3
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,10 @@ RUN apt-get update && \
3838
ros-humble-ament-cmake \
3939
ros-humble-ament-cmake-clang-format \
4040
ros-humble-moveit \
41+
ros-humble-ros2-control \
42+
ros-humble-ros2-controllers \
4143
ros-humble-joint-state-broadcaster \
44+
ros-humble-ament-cmake-clang-tidy \
4245
libignition-gazebo6-dev \
4346
ros-humble-joint-trajectory-controller \
4447
libpoco-dev \

‎CHANGELOG.md

+9
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,14 @@
11
# Changelog
22

3+
## 0.1.7 - 2023-11-10
4+
5+
Requires libfranka >= 0.12.1, required ROS 2 Humble
6+
7+
* franka\_hardware: joint position command inteface supported
8+
* franka\_hardware: controller initializer automatically acknowledges error, if arm is in reflex mode
9+
* franka\_example\_controllers: joint position example controller provided
10+
* franka\_example\_controllers: fix second start bug with the example controllers
11+
312
## 0.1.6 - 2023-11-03
413

514
Requires libfranka >= 0.12.1, required ROS 2 Humble

‎franka_bringup/config/controllers.yaml

+3
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,9 @@ controller_manager:
1111
joint_velocity_example_controller:
1212
type: franka_example_controllers/JointVelocityExampleController
1313

14+
joint_position_example_controller:
15+
type: franka_example_controllers/JointPositionExampleController
16+
1417
cartesian_velocity_example_controller:
1518
type: franka_example_controllers/CartesianVelocityExampleController
1619

‎franka_bringup/launch/joint_position_example_controller.launch.py

+77
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
# Copyright (c) 2021 Franka Emika GmbH
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
16+
from launch import LaunchDescription
17+
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
18+
from launch.launch_description_sources import PythonLaunchDescriptionSource
19+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
20+
from launch_ros.actions import Node
21+
from launch_ros.substitutions import FindPackageShare
22+
23+
24+
def generate_launch_description():
25+
robot_ip_parameter_name = 'robot_ip'
26+
load_gripper_parameter_name = 'load_gripper'
27+
use_fake_hardware_parameter_name = 'use_fake_hardware'
28+
fake_sensor_commands_parameter_name = 'fake_sensor_commands'
29+
use_rviz_parameter_name = 'use_rviz'
30+
31+
robot_ip = LaunchConfiguration(robot_ip_parameter_name)
32+
load_gripper = LaunchConfiguration(load_gripper_parameter_name)
33+
use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
34+
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
35+
use_rviz = LaunchConfiguration(use_rviz_parameter_name)
36+
37+
return LaunchDescription([
38+
DeclareLaunchArgument(
39+
robot_ip_parameter_name,
40+
description='Hostname or IP address of the robot.'),
41+
DeclareLaunchArgument(
42+
use_rviz_parameter_name,
43+
default_value='false',
44+
description='Visualize the robot in Rviz'),
45+
DeclareLaunchArgument(
46+
use_fake_hardware_parameter_name,
47+
default_value='false',
48+
description='Use fake hardware'),
49+
DeclareLaunchArgument(
50+
fake_sensor_commands_parameter_name,
51+
default_value='false',
52+
description="Fake sensor commands. Only valid when '{}' is true".format(
53+
use_fake_hardware_parameter_name)),
54+
DeclareLaunchArgument(
55+
load_gripper_parameter_name,
56+
default_value='true',
57+
description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
58+
'without an end-effector.'),
59+
60+
IncludeLaunchDescription(
61+
PythonLaunchDescriptionSource([PathJoinSubstitution(
62+
[FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
63+
launch_arguments={robot_ip_parameter_name: robot_ip,
64+
load_gripper_parameter_name: load_gripper,
65+
use_fake_hardware_parameter_name: use_fake_hardware,
66+
fake_sensor_commands_parameter_name: fake_sensor_commands,
67+
use_rviz_parameter_name: use_rviz
68+
}.items(),
69+
),
70+
71+
Node(
72+
package='controller_manager',
73+
executable='spawner',
74+
arguments=['joint_position_example_controller'],
75+
output='screen',
76+
),
77+
])

‎franka_bringup/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>franka_bringup</name>
5-
<version>0.1.6</version>
5+
<version>0.1.7</version>
66
<description>Package with launch files and run-time configurations for using Franka Emika research robots with ros2_control</description>
77
<maintainer email="[email protected]">Franka Emika GmbH</maintainer>
88
<license>Apache 2.0</license>

‎franka_description/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>franka_description</name>
5-
<version>0.1.6</version>
5+
<version>0.1.7</version>
66
<description>franka_description contains URDF files and meshes of Franka Emika robots</description>
77
<maintainer email="[email protected]">Franka Emika GmbH</maintainer>
88
<license>Apache 2.0</license>

‎franka_description/robots/panda_arm.ros2_control.xacro

+1
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
<param name="initial_position">${initial_position}</param>
2121
<command_interface name="effort"/>
2222
<command_interface name="velocity"/>
23+
<command_interface name="position"/>
2324
<state_interface name="position"/>
2425
<state_interface name="velocity"/>
2526
<state_interface name="effort"/>

‎franka_example_controllers/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@ add_library(
3131
src/gravity_compensation_example_controller.cpp
3232
src/joint_impedance_example_controller.cpp
3333
src/joint_velocity_example_controller.cpp
34+
src/joint_position_example_controller.cpp
3435
src/cartesian_velocity_example_controller.cpp
3536
src/elbow_example_controller.cpp
3637
src/model_example_controller.cpp

‎franka_example_controllers/franka_example_controllers.xml

+6
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,12 @@
1717
The joint velocity example controller moves joint 4 and 5 in a periodic movement.
1818
</description>
1919
</class>
20+
<class name="franka_example_controllers/JointPositionExampleController"
21+
type="franka_example_controllers::JointPositionExampleController" base_class_type="controller_interface::ControllerInterface">
22+
<description>
23+
The joint position example controller moves in a periodic movement.
24+
</description>
25+
</class>
2026
<class name="franka_example_controllers/CartesianVelocityExampleController"
2127
type="franka_example_controllers::CartesianVelocityExampleController" base_class_type="controller_interface::ControllerInterface">
2228
<description>

‎franka_example_controllers/include/franka_example_controllers/elbow_example_controller.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ class ElbowExampleController : public controller_interface::ControllerInterface
4747

4848
const bool k_elbow_activated_{true};
4949
std::vector<double> initial_cartesian_velocity_and_elbow;
50-
bool first_pass_{true};
50+
bool initialization_flag_{true};
5151
std::array<double, 2> initial_elbow_configuration_{0.0, 0.0};
5252
double elapsed_time_{0.0};
5353
const double traj_frequency_{0.001};

‎franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ class JointImpedanceExampleController : public controller_interface::ControllerI
4949
Vector7d dq_filtered_;
5050
Vector7d k_gains_;
5151
Vector7d d_gains_;
52-
rclcpp::Time start_time_;
52+
double elapsed_time_{0.0};
5353
void updateJointStates();
5454
};
5555

‎franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.hpp

+55
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
// Copyright (c) 2023 Franka Emika GmbH
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#pragma once
16+
17+
#include <string>
18+
19+
#include <Eigen/Eigen>
20+
#include <controller_interface/controller_interface.hpp>
21+
#include <rclcpp/rclcpp.hpp>
22+
#include "franka_semantic_components/franka_robot_state.hpp"
23+
24+
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
25+
26+
namespace franka_example_controllers {
27+
28+
/**
29+
* The joint position example controller moves in a periodic movement.
30+
*/
31+
class JointPositionExampleController : public controller_interface::ControllerInterface {
32+
public:
33+
[[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
34+
const override;
35+
[[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
36+
const override;
37+
controller_interface::return_type update(const rclcpp::Time& time,
38+
const rclcpp::Duration& period) override;
39+
CallbackReturn on_init() override;
40+
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
41+
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
42+
43+
private:
44+
std::string arm_id_;
45+
const int num_joints = 7;
46+
std::array<double, 7> initial_q_{0, 0, 0, 0, 0, 0, 0};
47+
const double trajectory_period{0.001};
48+
double elapsed_time_ = 0.0;
49+
std::string arm_id{"panda"};
50+
51+
bool initialization_flag_{true};
52+
rclcpp::Time start_time_;
53+
};
54+
55+
} // namespace franka_example_controllers

‎franka_example_controllers/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>franka_example_controllers</name>
5-
<version>0.1.6</version>
5+
<version>0.1.7</version>
66
<description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros2_control</description>
77
<maintainer email="[email protected]">Franka Emika GmbH</maintainer>
88
<license>Apache 2.0</license>

‎franka_example_controllers/src/cartesian_velocity_example_controller.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,7 @@ CallbackReturn CartesianVelocityExampleController::on_configure(
9696
CallbackReturn CartesianVelocityExampleController::on_activate(
9797
const rclcpp_lifecycle::State& /*previous_state*/) {
9898
franka_cartesian_velocity_->assign_loaned_command_interfaces(command_interfaces_);
99-
99+
elapsed_time_ = rclcpp::Duration(0, 0);
100100
return CallbackReturn::SUCCESS;
101101
}
102102

‎franka_example_controllers/src/elbow_example_controller.cpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ controller_interface::InterfaceConfiguration ElbowExampleController::state_inter
4242
controller_interface::return_type ElbowExampleController::update(
4343
const rclcpp::Time& /*time*/,
4444
const rclcpp::Duration& /*period*/) {
45-
if (first_pass_) {
45+
if (initialization_flag_) {
4646
// Get initial elbow configuration values
4747
if (!franka_cartesian_velocity_->getCommandedElbowConfiguration(initial_elbow_configuration_)) {
4848
RCLCPP_FATAL(get_node()->get_logger(),
@@ -51,11 +51,11 @@ controller_interface::return_type ElbowExampleController::update(
5151
return controller_interface::return_type::ERROR;
5252
}
5353

54-
first_pass_ = false;
54+
initialization_flag_ = false;
5555
}
5656
elapsed_time_ = elapsed_time_ + traj_frequency_;
5757

58-
double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_));
58+
double angle = M_PI / 15.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_));
5959

6060
std::array<double, 6> cartesian_velocity_command = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
6161
std::array<double, 2> elbow_command = {
@@ -103,7 +103,8 @@ CallbackReturn ElbowExampleController::on_configure(
103103
CallbackReturn ElbowExampleController::on_activate(
104104
const rclcpp_lifecycle::State& /*previous_state*/) {
105105
franka_cartesian_velocity_->assign_loaned_command_interfaces(command_interfaces_);
106-
106+
initialization_flag_ = true;
107+
elapsed_time_ = 0.0;
107108
return CallbackReturn::SUCCESS;
108109
}
109110

‎franka_example_controllers/src/joint_impedance_example_controller.cpp

+7-4
Original file line numberDiff line numberDiff line change
@@ -47,11 +47,12 @@ JointImpedanceExampleController::state_interface_configuration() const {
4747

4848
controller_interface::return_type JointImpedanceExampleController::update(
4949
const rclcpp::Time& /*time*/,
50-
const rclcpp::Duration& /*period*/) {
50+
const rclcpp::Duration& period) {
5151
updateJointStates();
5252
Vector7d q_goal = initial_q_;
53-
auto time = this->get_node()->now() - start_time_;
54-
double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time.seconds()));
53+
elapsed_time_ = elapsed_time_ + period.seconds();
54+
55+
double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * elapsed_time_));
5556
q_goal(3) += delta_angle;
5657
q_goal(4) += delta_angle;
5758

@@ -111,8 +112,10 @@ CallbackReturn JointImpedanceExampleController::on_configure(
111112
CallbackReturn JointImpedanceExampleController::on_activate(
112113
const rclcpp_lifecycle::State& /*previous_state*/) {
113114
updateJointStates();
115+
dq_filtered_.setZero();
114116
initial_q_ = q_;
115-
start_time_ = this->get_node()->now();
117+
elapsed_time_ = 0.0;
118+
116119
return CallbackReturn::SUCCESS;
117120
}
118121

0 commit comments

Comments
 (0)