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
No related branches found
No related tags found
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