Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +02:00
Code Releases Activity

added ur_controllers package

Browse Source
This commit is contained in:
Felix Mauch
2019-04-17 17:04:01 +02:00
parent be5553648d
commit f38ec92389
16 changed files with 1022 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
ur_controllers/include/ur_controllers/hardware_interface_adapter.h Normal file
View File

@@ -0,0 +1,84 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-18
*
*/
//----------------------------------------------------------------------
#ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
#define UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
#include <joint_trajectory_controller/hardware_interface_adapter.h>
#include "ur_controllers/scaled_joint_command_interface.h"
/**
* \brief Adapter for a position-controlled hardware interface. Forwards desired positions as commands.
*
* The following is an example configuration of a controller that uses this adapter.
* \code
* head_controller:
* type: "position_controllers/ScaledJointTrajectoryController"
* joints:
* - head_1_joint
* - head_2_joint
*
* constraints:
* goal_time: 0.6
* stopped_velocity_tolerance: 0.02
* head_1_joint: {trajectory: 0.05, goal: 0.02}
* head_2_joint: {trajectory: 0.05, goal: 0.02}
* stop_trajectory_duration: 0.5
* state_publish_rate: 25
* \endcode
*/
template <class State>
class HardwareInterfaceAdapter<ur_controllers::ScaledPositionJointInterface, State>
{
public:
HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
bool init(std::vector<ur_controllers::ScaledJointHandle>& joint_handles, ros::NodeHandle& /*controller_nh*/)
{
// Store pointer to joint handles
joint_handles_ptr_ = &joint_handles;
return true;
}
void starting(const ros::Time& /*time*/)
{
if (!joint_handles_ptr_) {return;}
// Semantic zero for commands
for (auto& jh : *joint_handles_ptr_)
{
jh.setCommand(jh.getPosition());
}
}
void stopping(const ros::Time& /*time*/) {}
void updateCommand(const ros::Time& /*time*/,
const ros::Duration& /*period*/,
const State& desired_state,
const State& /*state_error*/)
{
// Forward desired position to command
const unsigned int n_joints = joint_handles_ptr_->size();
for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
}
private:
std::vector<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
};
#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED

139
ur_controllers/include/ur_controllers/scaled_joint_command_interface.h Normal file
View File

@@ -0,0 +1,139 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright (C) 2012, hiDOF INC.
// Copyright (C) 2019, FZI Forschungszentrum Informatik (Scaling extension)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of hiDOF, Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
/// \author Wim Meeussen
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-18
*
*/
//----------------------------------------------------------------------
#ifndef UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED
#define UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED
#include <hardware_interface/internal/hardware_resource_manager.h>
#include <hardware_interface/joint_state_interface.h>
namespace ur_controllers
{
class ScaledJointHandle : public hardware_interface::JointStateHandle
{
public:
ScaledJointHandle() : cmd_(0), scaling_factor_(0)
{
}
/**
* \param js This joint's state handle
* \param cmd A pointer to the storage for this joint's output command
*/
ScaledJointHandle(const hardware_interface::JointStateHandle& js, double* cmd, double* scaling_factor)
: hardware_interface::JointStateHandle(js), cmd_(cmd), scaling_factor_(scaling_factor)
{
if (cmd_ == nullptr)
{
throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
"'. Command data pointer is null.");
}
if (scaling_factor_ == nullptr)
{
throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() +
"'. Scaling factor pointer is null.");
}
}
virtual ~ScaledJointHandle() = default;
void setCommand(double command)
{
assert(cmd_);
*cmd_ = command;
}
double getCommand() const
{
assert(cmd_);
return *cmd_;
}
void setScalingFactor(double scaling_factor)
{
assert(scaling_factor_);
*scaling_factor_ = scaling_factor;
}
double getScalingFactor() const
{
assert(scaling_factor_);
return *scaling_factor_;
}
private:
double* cmd_;
double* scaling_factor_;
};
/** \brief Hardware interface to support commanding an array of joints.
*
* This \ref HardwareInterface supports commanding the output of an array of
* named joints. Note that these commands can have any semantic meaning as long
* as they each can be represented by a single double, they are not necessarily
* effort commands. To specify a meaning to this command, see the derived
* classes like \ref EffortJointInterface etc.
*
* These interfaces provide an additional scaling factor implemented, e.g. by a robot's speed
* slider.
*
*
* \note Getting a joint handle through the getHandle() method \e will claim that resource.
*
*/
class ScaledJointCommandInterface
: public hardware_interface::HardwareResourceManager<ScaledJointHandle, hardware_interface::ClaimResources>
{
};
/// \ref ScaledJointCommandInterface for commanding effort-based joints.
class ScaledEffortJointInterface : public ScaledJointCommandInterface
{
};
/// \ref ScaledJointCommandInterface for commanding velocity-based joints.
class ScaledVelocityJointInterface : public ScaledJointCommandInterface
{
};
/// \ref ScaledJointCommandInterface for commanding position-based joints.
class ScaledPositionJointInterface : public ScaledJointCommandInterface
{
};
} // namespace ur_controllers
#endif // ifndef UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED

193
ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.h Normal file
View File

@@ -0,0 +1,193 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-18
*
*/
//----------------------------------------------------------------------
#ifndef UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED
#define UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED
#include "ur_controllers/hardware_interface_adapter.h"
#include <joint_trajectory_controller/joint_trajectory_controller.h>
namespace ur_controllers
{
template <class SegmentImpl, class HardwareInterface>
class ScaledJointTrajectoryController
: public joint_trajectory_controller::JointTrajectoryController<SegmentImpl, HardwareInterface>
{
public:
ScaledJointTrajectoryController() = default;
virtual ~ScaledJointTrajectoryController() = default;
void update(const ros::Time& time, const ros::Duration& period)
{
this->scaling_factor_ = this->joints_[0].getScalingFactor();
// Get currently followed trajectory
typename Base::TrajectoryPtr curr_traj_ptr;
this->curr_trajectory_box_.get(curr_traj_ptr);
typename Base::Trajectory& curr_traj = *curr_traj_ptr;
// Update time data
typename Base::TimeData time_data;
time_data.time = time; // Cache current time
time_data.period = ros::Duration(this->scaling_factor_ * period.toSec()); // Cache current control period
time_data.uptime = this->time_data_.readFromRT()->uptime + time_data.period; // Update controller uptime
ros::Time traj_time = this->time_data_.readFromRT()->uptime + period;
this->time_data_.writeFromNonRT(time_data); // TODO: Grrr, we need a lock-free data structure here!
// NOTE: It is very important to execute the two above code blocks in the specified sequence: first get current
// trajectory, then update time data. Hopefully the following paragraph sheds a bit of light on the rationale.
// The non-rt thread responsible for processing new commands enqueues trajectories that can start at the _next_
// control cycle (eg. zero start time) or later (eg. when we explicitly request a start time in the future).
// If we reverse the order of the two blocks above, and update the time data first; it's possible that by the time
// we fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts
// in the next control cycle, leaving the current cycle without a valid trajectory.
// Update current state and state error
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
this->current_state_.position[i] = this->joints_[i].getPosition();
this->current_state_.velocity[i] = this->joints_[i].getVelocity();
// There's no acceleration data available in a joint handle
typename Base::TrajectoryPerJoint::const_iterator segment_it =
sample(curr_traj[i], traj_time.toSec(), this->desired_joint_state_);
if (curr_traj[i].end() == segment_it)
{
// Non-realtime safe, but should never happen under normal operation
ROS_ERROR_NAMED(this->name_, "Unexpected error: No trajectory defined at current time. Please contact the "
"package "
"maintainer.");
return;
}
this->desired_state_.position[i] = this->desired_joint_state_.position[0];
this->desired_state_.velocity[i] = this->desired_joint_state_.velocity[0];
this->desired_state_.acceleration[i] = this->desired_joint_state_.acceleration[0];
;
this->state_joint_error_.position[0] =
angles::shortest_angular_distance(this->current_state_.position[i], this->desired_joint_state_.position[0]);
this->state_joint_error_.velocity[0] = this->desired_joint_state_.velocity[0] - this->current_state_.velocity[i];
this->state_joint_error_.acceleration[0] = 0.0;
this->state_error_.position[i] =
angles::shortest_angular_distance(this->current_state_.position[i], this->desired_joint_state_.position[0]);
this->state_error_.velocity[i] = this->desired_joint_state_.velocity[0] - this->current_state_.velocity[i];
this->state_error_.acceleration[i] = 0.0;
// Check tolerances
const typename Base::RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
if (rt_segment_goal && rt_segment_goal == this->rt_active_goal_)
{
// Check tolerances
if (time_data.uptime.toSec() < segment_it->endTime())
{
// Currently executing a segment: check path tolerances
const joint_trajectory_controller::SegmentTolerancesPerJoint<typename Base::Scalar>& joint_tolerances =
segment_it->getTolerances();
if (!checkStateTolerancePerJoint(this->state_joint_error_, joint_tolerances.state_tolerance))
{
if (this->verbose_)
{
ROS_ERROR_STREAM_NAMED(this->name_, "Path tolerances failed for joint: " << this->joint_names_[i]);
checkStateTolerancePerJoint(this->state_joint_error_, joint_tolerances.state_tolerance, true);
}
rt_segment_goal->preallocated_result_->error_code =
control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
this->rt_active_goal_.reset();
this->successful_joint_traj_.reset();
}
}
else if (segment_it == --curr_traj[i].end())
{
if (this->verbose_)
ROS_DEBUG_STREAM_THROTTLE_NAMED(1, this->name_,
"Finished executing last segment, checking goal tolerances");
// Controller uptime
const ros::Time uptime = this->time_data_.readFromRT()->uptime;
// Checks that we have ended inside the goal tolerances
const joint_trajectory_controller::SegmentTolerancesPerJoint<typename Base::Scalar>& tolerances =
segment_it->getTolerances();
const bool inside_goal_tolerances =
checkStateTolerancePerJoint(this->state_joint_error_, tolerances.goal_state_tolerance);
if (inside_goal_tolerances)
{
this->successful_joint_traj_[i] = 1;
}
else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance)
{
// Still have some time left to meet the goal state tolerances
}
else
{
if (this->verbose_)
{
ROS_ERROR_STREAM_NAMED(this->name_, "Goal tolerances failed for joint: " << this->joint_names_[i]);
// Check the tolerances one more time to output the errors that occurs
checkStateTolerancePerJoint(this->state_joint_error_, tolerances.goal_state_tolerance, true);
}
rt_segment_goal->preallocated_result_->error_code =
control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
this->rt_active_goal_.reset();
this->successful_joint_traj_.reset();
}
}
}
}
// If there is an active goal and all segments finished successfully then set goal as succeeded
typename Base::RealtimeGoalHandlePtr current_active_goal(this->rt_active_goal_);
if (current_active_goal && this->successful_joint_traj_.count() == this->joints_.size())
{
current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
current_active_goal->setSucceeded(current_active_goal->preallocated_result_);
current_active_goal.reset(); // do not publish feedback
this->rt_active_goal_.reset();
this->successful_joint_traj_.reset();
}
// Hardware interface adapter: Generate and send commands
this->hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period, this->desired_state_, this->state_error_);
// Set action feedback
if (current_active_goal)
{
current_active_goal->preallocated_feedback_->header.stamp = this->time_data_.readFromRT()->time;
current_active_goal->preallocated_feedback_->desired.positions = this->desired_state_.position;
current_active_goal->preallocated_feedback_->desired.velocities = this->desired_state_.velocity;
current_active_goal->preallocated_feedback_->desired.accelerations = this->desired_state_.acceleration;
current_active_goal->preallocated_feedback_->actual.positions = this->current_state_.position;
current_active_goal->preallocated_feedback_->actual.velocities = this->current_state_.velocity;
current_active_goal->preallocated_feedback_->error.positions = this->state_error_.position;
current_active_goal->preallocated_feedback_->error.velocities = this->state_error_.velocity;
current_active_goal->setFeedback(current_active_goal->preallocated_feedback_);
}
// Publish state
this->publishState(time_data.uptime);
}
protected:
using Base = joint_trajectory_controller::JointTrajectoryController<SegmentImpl, HardwareInterface>;
double scaling_factor_;
private:
/* data */
};
} // namespace ur_controllers
#endif // ifndef UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED

50
ur_controllers/include/ur_controllers/speed_scaling_interface.h Normal file
View File

@@ -0,0 +1,50 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-17
*
*/
//----------------------------------------------------------------------
#ifndef UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED
#define UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED
#include <hardware_interface/internal/hardware_resource_manager.h>
namespace ur_controllers
{
class SpeedScalingHandle
{
public:
SpeedScalingHandle() : name_(""), scaling_factor_(0){};
SpeedScalingHandle(const std::string& name, const double* scaling_factor)
: name_(name), scaling_factor_(scaling_factor){};
virtual ~SpeedScalingHandle() = default;
std::string getName() const
{
return name_;
}
const double* getScalingFactor() const
{
return scaling_factor_;
}
private:
std::string name_;
const double* scaling_factor_;
};
/** \brief Hardware interface to support reading the speed scaling factor. */
class SpeedScalingInterface : public hardware_interface::HardwareResourceManager<SpeedScalingHandle>
{
};
} // namespace ur_controllers
#endif // ifndef UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED

46
ur_controllers/include/ur_controllers/speed_scaling_state_controller.h Normal file
View File

@@ -0,0 +1,46 @@
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
// -- BEGIN LICENSE BLOCK ----------------------------------------------
// -- END LICENSE BLOCK ------------------------------------------------
//----------------------------------------------------------------------
/*!\file
*
* \author Felix Mauch mauch@fzi.de
* \date 2019-04-17
*
*/
//----------------------------------------------------------------------
#ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED
#define UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED
#include <controller_interface/controller.h>
#include <realtime_tools/realtime_publisher.h>
#include <std_msgs/Float64.h>
#include "ur_controllers/speed_scaling_interface.h"
namespace ur_controllers
{
class SpeedScalingStateController : public controller_interface::Controller<SpeedScalingInterface>
{
public:
SpeedScalingStateController() = default;
virtual ~SpeedScalingStateController() override = default;
virtual bool init(SpeedScalingInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
virtual void starting(const ros::Time& time) override;
virtual void update(const ros::Time& time, const ros::Duration& /*period*/) override;
virtual void stopping(const ros::Time& /*time*/) override;
private:
std::vector<SpeedScalingHandle> sensors_;
//TODO: We should use a better datatype later on
typedef std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64> > RtPublisherPtr;
std::vector<RtPublisherPtr> realtime_pubs_;
std::vector<ros::Time> last_publish_times_;
double publish_rate_;
};
} // namespace ur_controllers
#endif // ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED