Skip to content

Commit

Permalink
[JTC] Command final waypoint identically when traj_point_active_ptr_ …
Browse files Browse the repository at this point in the history
…is nullptr (#682)
  • Loading branch information
egordon authored Jun 26, 2023
1 parent cd40ec0 commit 9ce288b
Showing 1 changed file with 51 additions and 51 deletions.
102 changes: 51 additions & 51 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -241,57 +241,6 @@ controller_interface::return_type JointTrajectoryController::update(
}
}

// 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_)
{
if (use_closed_loop_pid_adapter_)
{
assign_interface_from_point(joint_command_interface_[3], tmp_command_);
}
else
{
assign_interface_from_point(joint_command_interface_[3], state_desired_.effort);
}
}

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

const auto active_goal = *rt_active_goal_.readFromRT();
if (active_goal)
{
Expand Down Expand Up @@ -359,6 +308,57 @@ controller_interface::return_type JointTrajectoryController::update(
set_hold_position();
RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation");
}

// 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 (traj_point_active_ptr_ && 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_)
{
if (traj_point_active_ptr_ && use_closed_loop_pid_adapter_)
{
assign_interface_from_point(joint_command_interface_[3], tmp_command_);
}
else
{
assign_interface_from_point(joint_command_interface_[3], state_desired_.effort);
}
}

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

Expand Down

0 comments on commit 9ce288b

Please sign in to comment.