From cddf89de418203b30cfce351e8820f1173735c18 Mon Sep 17 00:00:00 2001
From: Florian Walch <florian.walch@franka.de>
Date: Tue, 23 Jan 2018 15:12:04 +0100
Subject: [PATCH] Check starting position for basic examples, minor cleanup
---
.../cartesian_impedance_example_controller.h | 7 ++-
.../cartesian_pose_example_controller.h | 19 ++++----
.../cartesian_velocity_example_controller.h | 12 ++---
.../force_example_controller.h | 7 ++-
.../joint_impedance_example_controller.h | 9 ++--
.../joint_position_example_controller.h | 8 ++--
.../joint_velocity_example_controller.h | 12 ++---
.../model_example_controller.h | 7 ++-
franka_example_controllers/package.xml | 2 +-
...cartesian_impedance_example_controller.cpp | 2 -
.../src/cartesian_pose_example_controller.cpp | 39 ++++++++++++----
.../cartesian_velocity_example_controller.cpp | 42 ++++++++++++++----
.../src/force_example_controller.cpp | 2 -
.../joint_impedance_example_controller.cpp | 2 -
.../src/joint_position_example_controller.cpp | 21 ++++++---
.../src/joint_velocity_example_controller.cpp | 44 ++++++++++++++++---
.../src/model_example_controller.cpp | 7 ---
.../config/panda_arm_hand.srdf | 20 ++++-----
.../launch/move_to_start.launch | 2 +
19 files changed, 170 insertions(+), 94 deletions(-)
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
index e4ea62c..4b496cf 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
@@ -26,10 +26,9 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
hardware_interface::EffortJointInterface,
franka_hw::FrankaStateInterface> {
public:
- CartesianImpedanceExampleController();
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
- void starting(const ros::Time&);
- void update(const ros::Time&, const ros::Duration& period);
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
private:
std::string arm_id_;
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
index 59d5aa0..6a747df 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
@@ -7,6 +7,7 @@
#include <string>
#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
@@ -15,24 +16,22 @@
namespace franka_example_controllers {
-class CartesianPoseExampleController : public controller_interface::MultiInterfaceController<
- franka_hw::FrankaPoseCartesianInterface> {
+class CartesianPoseExampleController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface> {
public:
- CartesianPoseExampleController();
bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle&);
- void starting(const ros::Time&);
-
- void update(const ros::Time&, const ros::Duration& period);
+ ros::NodeHandle& /* controller_node_handle */) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
private:
std::string arm_id_;
franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
- std::array<double, 16> initial_pose_ = {
- {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
- ros::Duration elapsed_time_;
std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ ros::Duration elapsed_time_;
+ std::array<double, 16> initial_pose_{};
};
} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
index ccc8ff3..bbeba97 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
@@ -7,6 +7,7 @@
#include <controller_interface/multi_interface_controller.h>
#include <franka_hw/franka_cartesian_command_interface.h>
+#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
@@ -14,14 +15,15 @@
namespace franka_example_controllers {
class CartesianVelocityExampleController : public controller_interface::MultiInterfaceController<
- franka_hw::FrankaVelocityCartesianInterface> {
+ franka_hw::FrankaVelocityCartesianInterface,
+ franka_hw::FrankaStateInterface> {
public:
- CartesianVelocityExampleController();
bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle&);
- void update(const ros::Time&, const ros::Duration& period);
- void stopping(const ros::Time&);
+ ros::NodeHandle& /* controller_node_handle */) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+ void starting(const ros::Time&) override;
+ void stopping(const ros::Time&) override;
private:
std::string arm_id_;
diff --git a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
index 1100ff9..6427386 100644
--- a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
@@ -25,10 +25,9 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro
hardware_interface::EffortJointInterface,
franka_hw::FrankaStateInterface> {
public:
- ForceExampleController();
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
- void starting(const ros::Time&);
- void update(const ros::Time&, const ros::Duration& period);
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
private:
std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
index 240fbdd..c10a7da 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h
@@ -25,17 +25,16 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
hardware_interface::EffortJointInterface,
franka_hw::FrankaPoseCartesianInterface> {
public:
- JointImpedanceExampleController();
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
- void starting(const ros::Time&);
- void update(const ros::Time&, const ros::Duration& period);
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
private:
std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
std::vector<hardware_interface::JointHandle> joint_handles_;
std::array<double, 7> last_tau_d_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
- franka_hw::TriggerRate rate_trigger_;
+ franka_hw::TriggerRate rate_trigger_{1.0};
double radius_{0.1};
double acceleration_time_{2.0};
double vel_max_{0.05};
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
index 59938c6..2527cb3 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
@@ -20,16 +20,16 @@ class JointPositionExampleController : public controller_interface::MultiInterfa
JointPositionExampleController();
bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /*controller_node_handle*/);
- void starting(const ros::Time&);
- void update(const ros::Time&, const ros::Duration& period);
+ ros::NodeHandle& /* controller_node_handle */) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
private:
std::vector<std::string> joint_names_;
hardware_interface::PositionJointInterface* position_joint_interface_;
std::vector<hardware_interface::JointHandle> position_joint_handles_;
- std::array<double, 7> initial_pose_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
ros::Duration elapsed_time_;
+ std::array<double, 7> initial_pose_{};
};
} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
index 34bec83..d236007 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
@@ -6,6 +6,7 @@
#include <vector>
#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
@@ -14,14 +15,15 @@
namespace franka_example_controllers {
class JointVelocityExampleController : public controller_interface::MultiInterfaceController<
- hardware_interface::VelocityJointInterface> {
+ hardware_interface::VelocityJointInterface,
+ franka_hw::FrankaStateInterface> {
public:
- JointVelocityExampleController();
bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle&);
- void update(const ros::Time&, const ros::Duration& period);
- void stopping(const ros::Time&);
+ ros::NodeHandle& /* controller_node_handle */) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+ void starting(const ros::Time&) override;
+ void stopping(const ros::Time&) override;
private:
std::vector<std::string> joint_names_;
diff --git a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h b/franka_example_controllers/include/franka_example_controllers/model_example_controller.h
index fd1139a..c80ad74 100644
--- a/franka_example_controllers/include/franka_example_controllers/model_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/model_example_controller.h
@@ -19,17 +19,16 @@ namespace franka_example_controllers {
class ModelExampleController
: public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface> {
public:
- ModelExampleController();
- bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
- void update(const ros::Time&, const ros::Duration&);
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
+ void update(const ros::Time&, const ros::Duration&) override;
private:
franka_hw::FrankaStateInterface* franka_state_interface_;
std::unique_ptr<franka_hw::FrankaStateHandle> franka_state_handle_;
franka_hw::FrankaModelInterface* model_interface_;
std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
- franka_hw::TriggerRate rate_trigger_;
std::string arm_id_;
+ franka_hw::TriggerRate rate_trigger_{1.0};
};
} // namespace franka_example_controllers
diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml
index 9c37a4f..0ccc65d 100644
--- a/franka_example_controllers/package.xml
+++ b/franka_example_controllers/package.xml
@@ -2,7 +2,7 @@
<package format="2">
<name>franka_example_controllers</name>
<version>0.2.0</version>
- <description>The franka_example_controllers package</description>
+ <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros_control</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
index 5e96ecb..fec8a77 100644
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -13,8 +13,6 @@
namespace franka_example_controllers {
-CartesianImpedanceExampleController::CartesianImpedanceExampleController() = default;
-
bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& node_handle) {
std::vector<double> cartesian_stiffness_vector;
diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
index 441119d..f3e96ed 100644
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
@@ -15,12 +15,9 @@
namespace franka_example_controllers {
-CartesianPoseExampleController::CartesianPoseExampleController()
- : cartesian_pose_interface_(nullptr), elapsed_time_(0.0), cartesian_pose_handle_(nullptr) {}
-
bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /*controller_node_handle*/) {
+ ros::NodeHandle& /* controller_node_handle */) {
cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
if (cartesian_pose_interface_ == nullptr) {
ROS_ERROR(
@@ -39,18 +36,44 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
cartesian_pose_interface_->getHandle(arm_id_ + "_robot")));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
- "CartesianPoseExampleController: Exception getting cartesian handle: " << e.what());
+ "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
return false;
}
- elapsed_time_ = ros::Duration(0.0);
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("CartesianPoseExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id_ + "_robot");
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianPoseExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+
return true;
}
-void CartesianPoseExampleController::starting(const ros::Time& /*time*/) {
+void CartesianPoseExampleController::starting(const ros::Time& /* time */) {
initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ elapsed_time_ = ros::Duration(0.0);
}
-void CartesianPoseExampleController::update(const ros::Time& /*time*/,
+void CartesianPoseExampleController::update(const ros::Time& /* time */,
const ros::Duration& period) {
elapsed_time_ += period;
diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
index 6a12e1b..4593601 100644
--- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
@@ -15,14 +15,9 @@
namespace franka_example_controllers {
-CartesianVelocityExampleController::CartesianVelocityExampleController()
- : velocity_cartesian_interface_(nullptr),
- velocity_cartesian_handle_(nullptr),
- elapsed_time_(0.0) {}
-
bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /*controller_node_handle*/) {
+ ros::NodeHandle& /* controller_node_handle */) {
if (!root_node_handle.getParam("arm_id", arm_id_)) {
ROS_ERROR("CartesianVelocityExampleController: Could not get parameter arm_id");
return false;
@@ -41,15 +36,44 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
velocity_cartesian_interface_->getHandle(arm_id_ + "_robot")));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM(
- "CartesianVelocityExampleController: Exception getting cartesian handle: " << e.what());
+ "CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("CartesianVelocityExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id_ + "_robot");
+
+ std::array<double, 7> q_start = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "CartesianVelocityExampleController: Robot is not in the expected starting position "
+ "for "
+ "running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "CartesianVelocityExampleController: Exception getting state handle: " << e.what());
return false;
}
- elapsed_time_ = ros::Duration(0.0);
return true;
}
-void CartesianVelocityExampleController::update(const ros::Time& /*time*/,
+void CartesianVelocityExampleController::starting(const ros::Time& /* time */) {
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void CartesianVelocityExampleController::update(const ros::Time& /* time */,
const ros::Duration& period) {
elapsed_time_ += period;
diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp
index a44c55f..a06f22d 100644
--- a/franka_example_controllers/src/force_example_controller.cpp
+++ b/franka_example_controllers/src/force_example_controller.cpp
@@ -12,8 +12,6 @@
namespace franka_example_controllers {
-ForceExampleController::ForceExampleController() = default;
-
bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& node_handle) {
std::vector<std::string> joint_names;
diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp
index 991713c..2802528 100644
--- a/franka_example_controllers/src/joint_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp
@@ -23,8 +23,6 @@ std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
namespace franka_example_controllers {
-JointImpedanceExampleController::JointImpedanceExampleController() : rate_trigger_(1.0) {}
-
bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& node_handle) {
std::string arm_id;
diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp
index 8417ce4..3d680d1 100644
--- a/franka_example_controllers/src/joint_position_example_controller.cpp
+++ b/franka_example_controllers/src/joint_position_example_controller.cpp
@@ -13,12 +13,9 @@
namespace franka_example_controllers {
-JointPositionExampleController::JointPositionExampleController()
- : position_joint_interface_(nullptr), elapsed_time_(0.0) {}
-
bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /*controller_node_handle*/) {
+ ros::NodeHandle& /* controller_node_handle */) {
position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>();
if (position_joint_interface_ == nullptr) {
ROS_ERROR(
@@ -46,14 +43,26 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har
return false;
}
}
- elapsed_time_ = ros::Duration(0.0);
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(position_joint_handles_[i].getPosition() - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "JointPositionExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+
return true;
}
-void JointPositionExampleController::starting(const ros::Time& /*time*/) {
+void JointPositionExampleController::starting(const ros::Time& /* time */) {
for (size_t i = 0; i < 7; ++i) {
initial_pose_[i] = position_joint_handles_[i].getPosition();
}
+ elapsed_time_ = ros::Duration(0.0);
}
void JointPositionExampleController::update(const ros::Time& /*time*/,
diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp
index 5db177c..5b701c3 100644
--- a/franka_example_controllers/src/joint_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp
@@ -13,12 +13,9 @@
namespace franka_example_controllers {
-JointVelocityExampleController::JointVelocityExampleController()
- : velocity_joint_interface_(nullptr) {}
-
bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /*controller_node_handle*/) {
+ ros::NodeHandle& /* controller_node_handle */) {
velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>();
if (velocity_joint_interface_ == nullptr) {
ROS_ERROR(
@@ -46,11 +43,46 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
return false;
}
}
- elapsed_time_ = ros::Duration(0.0);
+
+ std::string arm_id;
+ if (!root_node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("JointVelocityExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "JointVelocityExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch panda_moveit_config move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM(
+ "JointVelocityExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+
return true;
}
-void JointVelocityExampleController::update(const ros::Time& /*time*/,
+void JointVelocityExampleController::starting(const ros::Time& /* time */) {
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void JointVelocityExampleController::update(const ros::Time& /* time */,
const ros::Duration& period) {
elapsed_time_ += period;
diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp
index 0bc0a87..1742520 100644
--- a/franka_example_controllers/src/model_example_controller.cpp
+++ b/franka_example_controllers/src/model_example_controller.cpp
@@ -29,13 +29,6 @@ std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
namespace franka_example_controllers {
-ModelExampleController::ModelExampleController()
- : franka_state_interface_(nullptr),
- franka_state_handle_(nullptr),
- model_interface_(nullptr),
- model_handle_(nullptr),
- rate_trigger_(1.0) {}
-
bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& node_handle) {
franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>();
diff --git a/panda_moveit_config/config/panda_arm_hand.srdf b/panda_moveit_config/config/panda_arm_hand.srdf
index d7697ec..56981c7 100644
--- a/panda_moveit_config/config/panda_arm_hand.srdf
+++ b/panda_moveit_config/config/panda_arm_hand.srdf
@@ -66,16 +66,16 @@
<joint name="panda_joint4" value="0" />
<joint name="panda_joint5" value="0" />
<joint name="panda_joint6" value="0" />
- <joint name="panda_joint7" value="0" />
+ <joint name="panda_joint7" value="0.785" />
</group_state>
<group_state name="ready" group="panda_arm">
<joint name="panda_joint1" value="0" />
- <joint name="panda_joint2" value="-0.2128" />
+ <joint name="panda_joint2" value="-0.785" />
<joint name="panda_joint3" value="0" />
- <joint name="panda_joint4" value="-1.928" />
+ <joint name="panda_joint4" value="-2.356" />
<joint name="panda_joint5" value="0" />
- <joint name="panda_joint6" value="1.7741" />
- <joint name="panda_joint7" value="0" />
+ <joint name="panda_joint6" value="1.571" />
+ <joint name="panda_joint7" value="0.785" />
</group_state>
<group_state name="home" group="panda_arm_hand">
<joint name="panda_joint1" value="0" />
@@ -84,16 +84,16 @@
<joint name="panda_joint4" value="0" />
<joint name="panda_joint5" value="0" />
<joint name="panda_joint6" value="0" />
- <joint name="panda_joint7" value="0.7918" />
+ <joint name="panda_joint7" value="0.785" />
</group_state>
<group_state name="ready" group="panda_arm_hand">
<joint name="panda_joint1" value="0" />
- <joint name="panda_joint2" value="-0.3912" />
+ <joint name="panda_joint2" value="-0.785" />
<joint name="panda_joint3" value="0" />
- <joint name="panda_joint4" value="-1.9161" />
+ <joint name="panda_joint4" value="-2.356" />
<joint name="panda_joint5" value="0" />
- <joint name="panda_joint6" value="1.5029" />
- <joint name="panda_joint7" value="0.7918" />
+ <joint name="panda_joint6" value="1.571" />
+ <joint name="panda_joint7" value="0.785" />
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="hand" parent_link="panda_hand" group="hand" parent_group="panda_arm_hand" />
diff --git a/panda_moveit_config/launch/move_to_start.launch b/panda_moveit_config/launch/move_to_start.launch
index a6a966e..1082569 100644
--- a/panda_moveit_config/launch/move_to_start.launch
+++ b/panda_moveit_config/launch/move_to_start.launch
@@ -2,10 +2,12 @@
<launch>
<arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" default="panda" />
+ <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch">
<arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
+ <arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
<arg name="arm_id" value="$(arg arm_id)" />
--
GitLab