Skip to content
Snippets Groups Projects
Commit 845d179b authored by Simon Gabl's avatar Simon Gabl
Browse files

Merge pull request #81 in SWDEV/franka_ros from elbow-commanding to develop

* commit '18a9a408':
  Update Changelog.
  Set empty elbow when only Cartesian velocities are commanded.
  Allow elbow commanding with Cartesian pose and velocity.
parents 33add06c 18a9a408
Branches
Tags
No related merge requests found
......@@ -5,6 +5,7 @@
Requires `libfranka` >= 0.3.0
* Added some missing includes to `franka_hw`
* Allow elbow commanding in Cartesian pose and Cartesian velocity interface.
## 0.4.0 - 2018-03-26
......
......@@ -49,6 +49,7 @@ catkin_package(
)
add_library(franka_example_controllers
src/elbow_example_controller.cpp
src/cartesian_pose_example_controller.cpp
src/cartesian_velocity_example_controller.cpp
src/joint_position_example_controller.cpp
......
......@@ -24,6 +24,10 @@ cartesian_pose_example_controller:
type: franka_example_controllers/CartesianPoseExampleController
arm_id: panda
elbow_example_controller:
type: franka_example_controllers/ElbowExampleController
arm_id: panda
cartesian_velocity_example_controller:
type: franka_example_controllers/CartesianVelocityExampleController
arm_id: panda
......
......@@ -19,6 +19,11 @@
A controller that executes a short motion based on cartesian velocities to demonstrate correct usage
</description>
</class>
<class name="franka_example_controllers/ElbowExampleController" type="franka_example_controllers::ElbowExampleController" base_class_type="controller_interface::ControllerBase">
<description>
A controller that executes a short motion based on cartesian poses and elbow to demonstrate correct usage
</description>
</class>
<class name="franka_example_controllers/ModelExampleController" type="franka_example_controllers::ModelExampleController" base_class_type="controller_interface::ControllerBase">
<description>
A controller that evaluates and prints the dynamic model of Franka
......
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
#include <array>
#include <memory>
#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>
#include <franka_hw/franka_cartesian_command_interface.h>
namespace franka_example_controllers {
class ElbowExampleController
: public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
franka_hw::FrankaStateInterface> {
public:
bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
void starting(const ros::Time&) override;
void update(const ros::Time&, const ros::Duration& period) override;
private:
franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
ros::Duration elapsed_time_;
std::array<double, 16> initial_pose_{};
std::array<double, 2> initial_elbow_{};
};
} // namespace franka_example_controllers
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" />
<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="load_gripper" value="$(arg load_gripper)" />
</include>
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="elbowf_example_controller"/>
<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
</launch>
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_example_controllers/elbow_example_controller.h>
#include <cmath>
#include <stdexcept>
#include <string>
#include <controller_interface/controller_base.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
namespace franka_example_controllers {
bool ElbowExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& node_handle) {
cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
if (cartesian_pose_interface_ == nullptr) {
ROS_ERROR("ElbowExampleController: Could not get Cartesian Pose interface from hardware");
return false;
}
std::string arm_id;
if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("ElbowExampleController: Could not get parameter arm_id");
return false;
}
try {
cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
cartesian_pose_interface_->getHandle(arm_id + "_robot")));
} catch (const hardware_interface::HardwareInterfaceException& e) {
ROS_ERROR_STREAM("ElbowExampleController: Exception getting Cartesian handle: " << e.what());
return false;
}
auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
if (state_interface == nullptr) {
ROS_ERROR("ElbowExampleController: 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(
"ElbowExampleController: Robot is not in the expected starting position for "
"running this example. Run `roslaunch franka_example_controllers 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("ElbowExampleController: Exception getting state handle: " << e.what());
return false;
}
return true;
}
void ElbowExampleController::starting(const ros::Time& /* time */) {
initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
initial_elbow_ = cartesian_pose_handle_->getRobotState().elbow_d;
elapsed_time_ = ros::Duration(0.0);
}
void ElbowExampleController::update(const ros::Time& /* time */, const ros::Duration& period) {
elapsed_time_ += period;
double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
auto elbow = initial_elbow_;
elbow[0] += angle;
cartesian_pose_handle_->setCommand(initial_pose_, elbow);
}
} // namespace franka_example_controllers
PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ElbowExampleController,
controller_interface::ControllerBase)
......@@ -21,17 +21,34 @@ class FrankaCartesianPoseHandle : public FrankaStateHandle {
*
* @param[in] franka_state_handle Robot state handle.
* @param[in] command A reference to the Cartesian pose command wrapped by this handle.
* @param[in] elbow A reference to the elbow wrapped by this handle.
*/
FrankaCartesianPoseHandle(const FrankaStateHandle& franka_state_handle,
std::array<double, 16>& command)
: FrankaStateHandle(franka_state_handle), command_(&command) {}
std::array<double, 16>& command,
std::array<double, 2>& elbow)
: FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}
/**
* Sets the given command.
*
* @param[in] command Command to set.
*/
void setCommand(const std::array<double, 16>& command) noexcept { *command_ = command; }
void setCommand(const std::array<double, 16>& command) noexcept {
*command_ = command;
*elbow_ = {};
}
/**
* Sets the given command.
*
* @param[in] command Command to set.
* @param[in] elbow Elbow to set.
*/
void setCommand(const std::array<double, 16>& command,
const std::array<double, 2>& elbow) noexcept {
*command_ = command;
*elbow_ = elbow;
}
/**
* Gets the current command.
......@@ -42,6 +59,7 @@ class FrankaCartesianPoseHandle : public FrankaStateHandle {
private:
std::array<double, 16>* command_;
std::array<double, 2>* elbow_;
};
/**
......@@ -61,17 +79,34 @@ class FrankaCartesianVelocityHandle : public FrankaStateHandle {
/**
* @param[in] franka_state_handle Robot state handle.
* @param[in] command A reference to the Cartesian velocity command wrapped by this handle.
* @param[in] elbow A reference to the elbow wrapped by this handle.
*/
FrankaCartesianVelocityHandle(const FrankaStateHandle& franka_state_handle,
std::array<double, 6>& command)
: FrankaStateHandle(franka_state_handle), command_(&command) {}
std::array<double, 6>& command,
std::array<double, 2>& elbow)
: FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}
/**
* Sets the given command.
*
* @param[in] command Command to set.
*/
void setCommand(std::array<double, 6>& command) noexcept {
*command_ = command;
*elbow_ = {};
}
/**
* Sets the given command.
*
* @param[in] command Command to set.
* @param[in] elbow Elbow to set.
*/
void setCommand(std::array<double, 6>& command) noexcept { *command_ = command; }
void setCommand(const std::array<double, 6>& command,
const std::array<double, 2>& elbow) noexcept {
*command_ = command;
*elbow_ = elbow;
}
/**
* Gets the current command.
......@@ -82,6 +117,7 @@ class FrankaCartesianVelocityHandle : public FrankaStateHandle {
private:
std::array<double, 6>* command_;
std::array<double, 2>* elbow_;
};
/**
......
......@@ -100,11 +100,11 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
FrankaStateHandle franka_state_handle(arm_id_ + "_robot", robot_state_);
franka_state_interface_.registerHandle(franka_state_handle);
FrankaCartesianPoseHandle franka_cartesian_pose_handle(franka_state_handle,
pose_cartesian_command_.O_T_EE);
FrankaCartesianPoseHandle franka_cartesian_pose_handle(
franka_state_handle, pose_cartesian_command_.O_T_EE, pose_cartesian_command_.elbow);
franka_pose_cartesian_interface_.registerHandle(franka_cartesian_pose_handle);
FrankaCartesianVelocityHandle franka_cartesian_velocity_handle(
franka_state_handle, velocity_cartesian_command_.O_dP_EE);
franka_state_handle, velocity_cartesian_command_.O_dP_EE, velocity_cartesian_command_.elbow);
franka_velocity_cartesian_interface_.registerHandle(franka_cartesian_velocity_handle);
registerInterface(&franka_state_interface_);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment