Skip to content

Commit

Permalink
Avoid "jumps" with states that have tracking error. All test are pass…
Browse files Browse the repository at this point in the history 
…ing but separatelly. Is there some kind of timeout?
  • Loading branch information
@destogl
destogl committed Mar 28, 2021
1 parent 325bdb6 commit 040f077
Show file tree
Hide file tree
Showing 4 changed files with 319 additions and 79 deletions.
29 changes: 29 additions & 0 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
hardware_interface::HW_IF_EFFORT,
};

// Parameters for some special cases, e.g. hydraulics powered robots
/// Read hardware states only when starting controller. This is useful when robot is not exactly
/// following the commanded trajectory.
bool hardware_state_has_offset_ = false;
trajectory_msgs::msg::JointTrajectoryPoint current_state_when_offset_;
/// Allow integration in goal trajectories to accept goals without position or velocity specified
bool allow_integration_in_goal_trajectories_ = false;

// The interfaces are defined as the types in 'allowed_interface_types_' member.
// For convenience, for each type the interfaces are ordered so that i-th position
// matches i-th index in joint_names_
Expand All @@ -131,6 +139,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
joint_state_interface_;

/// If true, a velocity feedforward term plus corrective PID term is used
// TODO(anyone): This flag is not used for now
// There should be PID-approach used as in ROS1:
// https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283
bool use_closed_loop_pid_adapter = false;

// TODO(karsten1987): eventually activate and deactive subscriber directly when its supported
Expand Down Expand Up @@ -207,7 +218,25 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
const JointTrajectoryPoint & current_state,
const JointTrajectoryPoint & state_error);

void read_state_from_hardware(JointTrajectoryPoint & state);

private:
void resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
{
point.positions.resize(size);
if (check_if_interface_type_exists(
state_interface_types_, hardware_interface::HW_IF_VELOCITY))
{
point.velocities.resize(size);
}
if (check_if_interface_type_exists(
state_interface_types_, hardware_interface::HW_IF_ACCELERATION))
{
point.accelerations.resize(size);
}
}

bool check_if_interface_type_exists(
const std::vector<std::string> & interface_type_list, const std::string & interface_type)
{
Expand Down
106 changes: 60 additions & 46 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,10 @@ JointTrajectoryController::init(const std::string & controller_name)
node_->declare_parameter<double>("state_publish_rate", 50.0);
node_->declare_parameter<double>("action_monitor_rate", 20.0);
node_->declare_parameter<bool>("allow_partial_joints_goal", allow_partial_joints_goal_);
node_->declare_parameter<bool>(
"hardware_state_has_offset", hardware_state_has_offset_);
node_->declare_parameter<bool>(
"allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_);
node_->declare_parameter<double>("constraints.stopped_velocity_tolerance", 0.01);
node_->declare_parameter<double>("constraints.goal_time", 0.0);

Expand Down Expand Up @@ -116,21 +120,6 @@ JointTrajectoryController::update()
return controller_interface::return_type::SUCCESS;
}

auto resize_joint_trajectory_point =
[&](trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
{
point.positions.resize(size);
if (check_if_interface_type_exists(
state_interface_types_, hardware_interface::HW_IF_VELOCITY))
{
point.velocities.resize(size);
}
if (check_if_interface_type_exists(
state_interface_types_, hardware_interface::HW_IF_ACCELERATION))
{
point.accelerations.resize(size);
}
};
auto compute_error_for_joint = [&](JointTrajectoryPoint & error, int index,
const JointTrajectoryPoint & current, const JointTrajectoryPoint & desired)
{
Expand Down Expand Up @@ -159,20 +148,14 @@ JointTrajectoryController::update()
if (current_external_msg != *new_external_msg) {
fill_partial_goal(*new_external_msg);
sort_to_local_joint_order(*new_external_msg);
// TODO(denis): Add here integration of position and velocity
traj_external_point_ptr_->update(*new_external_msg);
}

JointTrajectoryPoint state_current, state_desired, state_error;
const auto joint_num = joint_names_.size();
resize_joint_trajectory_point(state_current, joint_num);

auto assign_point_from_interface = [&, joint_num](
std::vector<double> & trajectory_point_interface, const auto & joint_inteface)
{
for (auto index = 0ul; index < joint_num; ++index) {
trajectory_point_interface[index] = joint_inteface[index].get().get_value();
}
};
// TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not
// changed, but its value only?
auto assign_interface_from_point = [&, joint_num](
Expand All @@ -185,35 +168,19 @@ JointTrajectoryController::update()

// current state update
state_current.time_from_start.set__sec(0);

// Assign values from the hardware
// Position states always exist
assign_point_from_interface(state_current.positions, joint_state_interface_[0]);
// velocity and acceleration states are optional
if (check_if_interface_type_exists(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) {
assign_point_from_interface(state_current.velocities, joint_state_interface_[1]);
// Acceleration is used only in combination with velocity
if (check_if_interface_type_exists(
state_interface_types_,
hardware_interface::HW_IF_ACCELERATION))
{
assign_point_from_interface(state_current.accelerations, joint_state_interface_[2]);
} else {
// Make empty so the property is ignored during interpolation
state_current.accelerations.clear();
}
} else {
// Make empty so the property is ignored during interpolation
state_current.velocities.clear();
state_current.accelerations.clear();
}
read_state_from_hardware(state_current);

// currently carrying out a trajectory
if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg() == false) {
// if sampling the first time, set the point before you sample
if (!(*traj_point_active_ptr_)->is_sampled_already()) {
(*traj_point_active_ptr_)->set_point_before_trajectory_msg(
node_->now(), state_current);
if (hardware_state_has_offset_) {
(*traj_point_active_ptr_)->set_point_before_trajectory_msg(
node_->now(), current_state_when_offset_);
} else {
(*traj_point_active_ptr_)->set_point_before_trajectory_msg(
node_->now(), state_current);
}
}
resize_joint_trajectory_point(state_error, joint_num);

Expand Down Expand Up @@ -269,6 +236,9 @@ JointTrajectoryController::update()
}
}

// store command as state when hardware state has tracking offset
current_state_when_offset_ = state_desired;

if (rt_active_goal_) {
// send feedback
auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
Expand Down Expand Up @@ -330,6 +300,40 @@ JointTrajectoryController::update()
return controller_interface::return_type::SUCCESS;
}

void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state)
{
const auto joint_num = joint_names_.size();
auto assign_point_from_interface = [&, joint_num](
std::vector<double> & trajectory_point_interface, const auto & joint_inteface)
{
for (auto index = 0ul; index < joint_num; ++index) {
trajectory_point_interface[index] = joint_inteface[index].get().get_value();
}
};

// Assign values from the hardware
// Position states always exist
assign_point_from_interface(state.positions, joint_state_interface_[0]);
// velocity and acceleration states are optional
if (check_if_interface_type_exists(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) {
assign_point_from_interface(state.velocities, joint_state_interface_[1]);
// Acceleration is used only in combination with velocity
if (check_if_interface_type_exists(
state_interface_types_,
hardware_interface::HW_IF_ACCELERATION))
{
assign_point_from_interface(state.accelerations, joint_state_interface_[2]);
} else {
// Make empty so the property is ignored during interpolation
state.accelerations.clear();
}
} else {
// Make empty so the property is ignored during interpolation
state.velocities.clear();
state.accelerations.clear();
}
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &)
{
Expand Down Expand Up @@ -487,6 +491,12 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &)

default_tolerances_ = get_segment_tolerances(*node_, joint_names_);

// Read parameters customizing controller for special cases
hardware_state_has_offset_ =
node_->get_parameter("hardware_state_has_offset").get_value<bool>();
allow_integration_in_goal_trajectories_ =
node_->get_parameter("allow_integration_in_goal_trajectories").get_value<bool>();

// subscriber callback
// non realtime
// TODO(karsten): check if traj msg and point time are valid
Expand Down Expand Up @@ -653,6 +663,10 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &)
traj_point_active_ptr_ = &traj_external_point_ptr_;
last_state_publish_time_ = node_->now();

// Initialize current state storage if hardware state has tracking offset
resize_joint_trajectory_point(current_state_when_offset_, joint_names_.size());
read_state_from_hardware(current_state_when_offset_);

// TODO(karsten1987): activate subscriptions of subscriber
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
Expand Down
Loading

0 comments on commit 040f077

Please sign in to comment.