Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added a test that sjtc correctly aborts on violation of constraints #811

Merged
merged 2 commits into from
Sep 22, 2023
Merged

Added a test that sjtc correctly aborts on violation of constraints #811

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5813de6
Added a test that sjtc correctly aborts on violation of constraints
fmauch Sep 20, 2023
789de22
Update sjtc to newest upstream API
fmauch Sep 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
226 changes: 121 additions & 105 deletions ur_controllers/src/scaled_joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat
}

controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
const rclcpp::Duration& /*period*/)
const rclcpp::Duration& period)
{
if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) {
scaling_factor_ = state_interfaces_.back().get_value();
Expand All @@ -87,20 +87,17 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
return controller_interface::return_type::OK;
}

auto resize_joint_trajectory_point = [&](trajectory_msgs::msg::JointTrajectoryPoint& point, size_t size) {
point.positions.resize(size);
if (has_velocity_state_interface_) {
point.velocities.resize(size);
}
if (has_acceleration_state_interface_) {
point.accelerations.resize(size);
}
};
auto compute_error_for_joint = [&](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current,
const JointTrajectoryPoint& desired) {
// error defined as the difference between current and desired
error.positions[index] = angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
if (has_velocity_state_interface_ && has_velocity_command_interface_) {
if (normalize_joint_error_[index]) {
// if desired, the shortest_angular_distance is calculated, i.e., the error is
// normalized between -pi<error<pi
error.positions[index] = angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
} else {
error.positions[index] = desired.positions[index] - current.positions[index];
}
if (has_velocity_state_interface_ && (has_velocity_command_interface_ || has_effort_command_interface_)) {
error.velocities[index] = desired.velocities[index] - current.velocities[index];
}
if (has_acceleration_state_interface_ && has_acceleration_command_interface_) {
Expand All @@ -114,103 +111,120 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
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);
// set the active trajectory pointer to the new goal
traj_point_active_ptr_ = &traj_external_point_ptr_;
}

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

// current state update
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();
}
};
auto assign_interface_from_point = [&, joint_num](auto& joint_inteface,
const std::vector<double>& trajectory_point_interface) {
for (auto index = 0ul; index < joint_num; ++index) {
joint_inteface[index].get().set_value(trajectory_point_interface[index]);
// 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 = [&](auto& joint_interface, const std::vector<double>& trajectory_point_interface) {
for (size_t index = 0; index < dof_; ++index) {
joint_interface[index].get().set_value(trajectory_point_interface[index]);
}
};

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 (has_velocity_state_interface_) {
assign_point_from_interface(state_current.velocities, joint_state_interface_[1]);
// Acceleration is used only in combination with velocity
if (has_acceleration_state_interface_) {
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();
}
// current state update
state_current_.time_from_start.set__sec(0);
read_state_from_hardware(state_current_);

// currently carrying out a trajectory
if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) {
// Main Speed scaling difference...
// Adjust time with scaling factor
TimeData time_data;
time_data.time = time;
rcl_duration_value_t period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * period);
rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * t_period);
time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(period);
rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period);
time_data_.writeFromNonRT(time_data);

bool first_sample = 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(traj_time, state_current);
first_sample = true;
if (params_.open_loop_control) {
(*traj_point_active_ptr_)->set_point_before_trajectory_msg(traj_time, last_commanded_state_);
} else {
(*traj_point_active_ptr_)->set_point_before_trajectory_msg(traj_time, state_current_);
}
}
resize_joint_trajectory_point(state_error, joint_num);

// find segment for current timestamp
joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr;
const bool valid_point =
(*traj_point_active_ptr_)
->sample(traj_time,
joint_trajectory_controller::interpolation_methods::InterpolationMethod::VARIABLE_DEGREE_SPLINE,
state_desired, start_segment_itr, end_segment_itr);
->sample(traj_time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);

if (valid_point) {
bool abort = false;
bool tolerance_violated_while_moving = false;
bool outside_goal_tolerance = false;
bool within_goal_time = true;
double time_difference = 0.0;
const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end();

// set values for next hardware write()
if (has_position_command_interface_) {
assign_interface_from_point(joint_command_interface_[0], state_desired.positions);
}
if (has_velocity_command_interface_) {
assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
}
if (has_acceleration_command_interface_) {
assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations);
}

for (size_t index = 0; index < joint_num; ++index) {
// set values for next hardware write()
compute_error_for_joint(state_error, index, state_current, state_desired);
// Check state/goal tolerance
for (size_t index = 0; index < dof_; ++index) {
compute_error_for_joint(state_error_, index, state_current_, state_desired_);

if (before_last_point &&
!check_state_tolerance_per_joint(state_error, index, default_tolerances_.state_tolerance[index], true)) {
abort = true;
// Always check the state tolerance on the first sample in case the first sample
// is the last point
if ((before_last_point || first_sample) &&
!check_state_tolerance_per_joint(state_error_, index, default_tolerances_.state_tolerance[index], false)) {
tolerance_violated_while_moving = true;
}
// past the final point, check that we end up inside goal tolerance
if (!before_last_point && !check_state_tolerance_per_joint(
state_error, index, default_tolerances_.goal_state_tolerance[index], true)) {
state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) {
outside_goal_tolerance = true;

if (default_tolerances_.goal_time_tolerance != 0.0) {
// if we exceed goal_time_tolerance set it to aborted
const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time();
const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;

time_difference = time.seconds() - traj_end.seconds();

if (time_difference > default_tolerances_.goal_time_tolerance) {
within_goal_time = false;
}
}
}
}

// set values for next hardware write() if tolerance is met
if (!tolerance_violated_while_moving && within_goal_time) {
if (use_closed_loop_pid_adapter_) {
// Update PIDs
for (auto i = 0ul; i < dof_; ++i) {
tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
pids_[i]->computeCommand(state_error_.positions[i], state_error_.velocities[i],
(uint64_t)period.nanoseconds());
}
}

// set values for next hardware write()
if (has_position_command_interface_) {
assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
}
if (has_velocity_command_interface_) {
if (use_closed_loop_pid_adapter_) {
assign_interface_from_point(joint_command_interface_[1], tmp_command_);
} else {
assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
}
}
if (has_acceleration_command_interface_) {
assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
}
if (has_effort_command_interface_) {
assign_interface_from_point(joint_command_interface_[3], tmp_command_);
}

// store the previous command. Used in open-loop control mode
last_commanded_state_ = state_desired_;
}

const auto active_goal = *rt_active_goal_.readFromRT();
Expand All @@ -220,58 +234,60 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
feedback->header.stamp = time;
feedback->joint_names = params_.joints;

feedback->actual = state_current;
feedback->desired = state_desired;
feedback->error = state_error;
feedback->actual = state_current_;
feedback->desired = state_desired_;
feedback->error = state_error_;
active_goal->setFeedback(feedback);

// check abort
if (abort || outside_goal_tolerance) {
if (tolerance_violated_while_moving) {
set_hold_position();
auto result = std::make_shared<FollowJTrajAction::Result>();

if (abort) {
RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation");
result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
} else if (outside_goal_tolerance) {
RCLCPP_WARN(get_node()->get_logger(), "Aborted due to goal tolerance violation");
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
}
RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation");
result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
active_goal->setAborted(result);
// TODO(matthew-reynolds): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
}
// remove the active trajectory pointer so that we stop commanding the hardware
traj_point_active_ptr_ = nullptr;

// check goal tolerance
if (!before_last_point) {
// check goal tolerance
} else if (!before_last_point) {
if (!outside_goal_tolerance) {
auto res = std::make_shared<FollowJTrajAction::Result>();
res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
active_goal->setSucceeded(res);
// TODO(matthew-reynolds): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
// remove the active trajectory pointer so that we stop commanding the hardware
traj_point_active_ptr_ = nullptr;

RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!");
} else if (default_tolerances_.goal_time_tolerance != 0.0) {
// if we exceed goal_time_toleralance set it to aborted
const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time();
const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;

// TODO(anyone): This will break in speed scaling we have to discuss how to handle the goal
// time when the robot scales itself down.
const double difference = time.seconds() - traj_end.seconds();
if (difference > default_tolerances_.goal_time_tolerance) {
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
active_goal->setAborted(result);
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds",
difference);
}
} else if (!within_goal_time) {
set_hold_position();
auto result = std::make_shared<FollowJTrajAction::Result>();
result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
active_goal->setAborted(result);
// TODO(matthew-reynolds): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds",
time_difference);
}
// else, run another cycle while waiting for outside_goal_tolerance
// to be satisfied or violated within the goal_time_tolerance
}
} else if (tolerance_violated_while_moving) {
set_hold_position();
RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation");
}
}
}

publish_state(state_desired, state_current, state_error);
publish_state(state_desired_, state_current_, state_error_);
return controller_interface::return_type::OK;
}

Expand Down
Loading
Loading