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

F#182 partial trajectory mod #185

Closed
wants to merge 18 commits into from
Closed

F#182 partial trajectory mod #185

Show file tree
Hide file tree
Changes from 12 commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
f4371b4
moving but only with one goal
Jul 23, 2015
38f6a1f
working with one goal, still needs to set the goal handlers correctly
Jul 24, 2015
57c7bcd
Trial with vector of goals but it turns out that the action server ca…
Jul 29, 2015
dc156d5
Partial trajectory working. Needs cleaning and check tolerances
Jul 30, 2015
c54846f
Working and cleaned
Jul 30, 2015
c53f577
Working and cleaned
Jul 30, 2015
6b8aa38
Changes made after modifing tests
Aug 3, 2015
37d0326
working on tests
Aug 3, 2015
093104b
Fixed test and all passed
Aug 3, 2015
324f851
Cleaning
Aug 3, 2015
36d6bc5
Fixed the import of new trajectories with accelerations empty and a test
Aug 7, 2015
2d939d7
Small fix to error message
Aug 11, 2015
af5c237
Fixed comments
Aug 17, 2015
eebbc96
Removed permutation from vector class and fixed tests
Aug 18, 2015
967d316
Fixed comment about assigning goal handle only to segments after curr…
Aug 18, 2015
a442849
Fixed setting of success to goal handle when all joints succeeded
Aug 19, 2015
b82cf4e
Added rosparam to allow partial joints goals
Aug 19, 2015
e19db70
added tests for partial joint goals
Aug 20, 2015
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
241 changes: 161 additions & 80 deletions joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
View file
Open in desktop

Large diffs are not rendered by default.

33 changes: 3 additions & 30 deletions ...t_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -166,8 +166,10 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;

typedef JointTrajectorySegment<SegmentImpl> Segment;
typedef std::vector<Segment> Trajectory;
typedef std::vector<Segment> TrajectoryPerJoint;
typedef std::vector<TrajectoryPerJoint> Trajectory;
typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
typedef boost::shared_ptr<TrajectoryPerJoint> TrajectoryPerJointPtr;
typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
typedef typename Segment::Scalar Scalar;

Expand Down Expand Up @@ -197,8 +199,6 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
typename Segment::State current_state_; ///< Preallocated workspace variable.
typename Segment::State desired_state_; ///< Preallocated workspace variable.
typename Segment::State state_error_; ///< Preallocated workspace variable.
typename Segment::State hold_start_state_; ///< Preallocated workspace variable.
typename Segment::State hold_end_state_; ///< Preallocated workspace variable.

realtime_tools::RealtimeBuffer<TimeData> time_data_;

Expand Down Expand Up @@ -241,33 +241,6 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
*/
void setHoldPosition(const ros::Time& time);

/**
* \brief Check path tolerances.
*
* If path tolerances are violated, the currently active action goal will be aborted.
*
* \param state_error Error between the current and desired trajectory states.
* \param segment Currently active trajectory segment.
*
* \pre \p segment is associated to the active goal handle.
**/
void checkPathTolerances(const typename Segment::State& state_error,
const Segment& segment);

/**
* \brief Check goal tolerances.
*
* If goal tolerances are fulfilled, the currently active action goal will be considered successful.
* If they are violated, the action goal will be aborted.
*
* \param state_error Error between the current and desired trajectory states.
* \param segment Currently active trajectory segment.
*
* \pre \p segment is associated to the active goal handle.
**/
void checkGoalTolerances(const typename Segment::State& state_error,
const Segment& segment);

};

} // namespace
Expand Down
245 changes: 132 additions & 113 deletions ...jectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,60 +179,6 @@ preemptActiveGoal()
}
}

template <class SegmentImpl, class HardwareInterface>
inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
checkPathTolerances(const typename Segment::State& state_error,
const Segment& segment)
{
const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
if (!checkStateTolerance(state_error, tolerances.state_tolerance))
{
rt_segment_goal->preallocated_result_->error_code =
control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
rt_active_goal_.reset();
}
}

template <class SegmentImpl, class HardwareInterface>
inline void JointTrajectoryController<SegmentImpl, HardwareInterface>::
checkGoalTolerances(const typename Segment::State& state_error,
const Segment& segment)
{
// Controller uptime
const ros::Time uptime = time_data_.readFromRT()->uptime;

// Checks that we have ended inside the goal tolerances
const RealtimeGoalHandlePtr rt_segment_goal = segment.getGoalHandle();
const SegmentTolerances<Scalar>& tolerances = segment.getTolerances();
const bool inside_goal_tolerances = checkStateTolerance(state_error, tolerances.goal_state_tolerance);

if (inside_goal_tolerances)
{
rt_segment_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
rt_segment_goal->setSucceeded(rt_segment_goal->preallocated_result_);
rt_active_goal_.reset();
}
else if (uptime.toSec() < segment.endTime() + tolerances.goal_time_tolerance)
{
// Still have some time left to meet the goal state tolerances
}
else
{
if (verbose_)
{
ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed");
// Check the tolerances one more time to output the errors that occures
checkStateTolerance(state_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_);
rt_active_goal_.reset();
}
}

template <class SegmentImpl, class HardwareInterface>
JointTrajectoryController<SegmentImpl, HardwareInterface>::
JointTrajectoryController()
Expand Down Expand Up @@ -347,11 +293,19 @@ bool JointTrajectoryController<SegmentImpl, HardwareInterface>::init(HardwareInt
current_state_ = typename Segment::State(n_joints);
desired_state_ = typename Segment::State(n_joints);
state_error_ = typename Segment::State(n_joints);
hold_start_state_ = typename Segment::State(n_joints);
hold_end_state_ = typename Segment::State(n_joints);

Segment hold_segment(0.0, current_state_, 0.0, current_state_);
hold_trajectory_ptr_->resize(1, hold_segment);
// Initialize trajectory with all joints
typename Segment::State current_joint_state_ = typename Segment::State(1);
for (unsigned int i = 0; i < n_joints; ++i)
{
current_joint_state_.position[0]= current_state_.position[i];
current_joint_state_.velocity[0]= current_state_.velocity[i];
Segment hold_segment(0.0, current_joint_state_, 0.0, current_joint_state_);

TrajectoryPerJoint joint_segment;
joint_segment.resize(1, hold_segment);
hold_trajectory_ptr_->push_back(joint_segment);
}

{
state_publisher_->lock();
Expand Down Expand Up @@ -393,48 +347,100 @@ update(const ros::Time& time, const ros::Duration& period)
// 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 desired state: sample trajectory at current time
typename Trajectory::const_iterator segment_it = sample(curr_traj, time_data.uptime.toSec(), desired_state_);
if (curr_traj.end() == segment_it)
{
// Non-realtime safe, but should never happen under normal operation
ROS_ERROR_NAMED(name_,
"Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
return;
}
std::vector<bool> successful_joint_traj;
successful_joint_traj.resize(joints_.size(), false);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Dynamic memory allocations are disallowed in the real-time path.

In controllers, computation takes place in two places:

  • update method: contains periodic, real-time computation.
  • ROS API callbacks: contains asynchronous, non real-time computation.

When implementing the update method, one has to be very careful not to break determinism and real-time friendliness.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can likely make the method real-time safe by pre-allocating resources on init, as the sizes are known at initialization time.

I forgot to say above that starting and stopping are also in the real-time path.


// Update current state and state error
for (unsigned int i = 0; i < joints_.size(); ++i)
{
typename Segment::State desired_joint_state_ = typename Segment::State(1);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Dynamic allocations are taking place here and in the line below as well.

typename Segment::State state_joint_error_= typename Segment::State(1);

current_state_.position[i] = joints_[i].getPosition();
current_state_.velocity[i] = joints_[i].getVelocity();
// There's no acceleration data available in a joint handle

state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];
state_error_.velocity[i] = desired_state_.velocity[i] - current_state_.velocity[i];
typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], time_data.uptime.toSec(), desired_joint_state_);
if (curr_traj[i].end() == segment_it)
{
// Non-realtime safe, but should never happen under normal operation
ROS_ERROR_NAMED(name_,
"Unexpected error: No trajectory defined at current time. Please contact the package maintainer.");
return;
}
desired_state_.position[i] = desired_joint_state_.position[0];
desired_state_.velocity[i] = desired_joint_state_.velocity[0];
desired_state_.acceleration[i] = 0.0;

state_joint_error_.position[0] = desired_joint_state_.position[0] - current_state_.position[i];
state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[i];
state_joint_error_.acceleration[0] = 0.0;

state_error_.position[i] = desired_joint_state_.position[0] - current_state_.position[i];
state_error_.velocity[i] = desired_joint_state_.velocity[0] - current_state_.velocity[i];
state_error_.acceleration[i] = 0.0;
}

// Check tolerances if segment corresponds to currently active action goal
const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
{
// Check tolerances
if (time_data.uptime.toSec() < segment_it->endTime())
//Check tolerances
const RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle();
if (rt_segment_goal && rt_segment_goal == rt_active_goal_)
{
// Currently executing a segment: check path tolerances
checkPathTolerances(state_error_,
*segment_it);
// Check tolerances
if (time_data.uptime.toSec() < segment_it->endTime())
{
// Currently executing a segment: check path tolerances
const SegmentTolerancesPerJoint<Scalar>& joint_tolerances = segment_it->getTolerances();
if (!checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance))
{
rt_segment_goal->preallocated_result_->error_code =
control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
rt_active_goal_.reset();
}
}
else if (segment_it == --curr_traj[i].end())
{
if (verbose_)
ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segment, checking goal tolerances");

// Controller uptime
const ros::Time uptime = time_data_.readFromRT()->uptime;

// Checks that we have ended inside the goal tolerances
const SegmentTolerancesPerJoint<Scalar>& tolerances = segment_it->getTolerances();
const bool inside_goal_tolerances = checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance);

if (inside_goal_tolerances)
{
successful_joint_traj[i] = true;

}
else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance)
{
// Still have some time left to meet the goal state tolerances
}
else
{
if (verbose_)
{
ROS_ERROR_STREAM_NAMED(name_,"Goal tolerances failed for joint: "<< joint_names_[i]);
// Check the tolerances one more time to output the errors that occurs
checkStateTolerancePerJoint(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_);
rt_active_goal_.reset();
}
}
}
else if (segment_it == --curr_traj.end())
{
if (verbose_)
ROS_DEBUG_STREAM_THROTTLE_NAMED(1,name_,"Finished executing last segement, checking goal tolerances");
}

// Finished executing the LAST segment: check goal tolerances
checkGoalTolerances(state_error_,
*segment_it);
}
//If all segments finished successfully then set goal as succeeded
if (std::find(successful_joint_traj.begin(), successful_joint_traj.end(), false) == successful_joint_traj.end())
{
rt_active_goal_->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
rt_active_goal_->setSucceeded(rt_active_goal_->preallocated_result_);
rt_active_goal_.reset();
}

// Hardware interface adapter: Generate and send commands
Expand Down Expand Up @@ -526,7 +532,7 @@ template <class SegmentImpl, class HardwareInterface>
void JointTrajectoryController<SegmentImpl, HardwareInterface>::
goalCB(GoalHandle gh)
{
ROS_DEBUG_STREAM_NAMED(name_,"Recieved new action goal");
ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal");

// Precondition: Running controller
if (!this->isRunning())
Expand All @@ -540,7 +546,7 @@ goalCB(GoalHandle gh)

// Goal should specify all controller joints (they can be ordered differently). Reject if this is not the case
using internal::permutation;
std::vector<unsigned int> permutation_vector = permutation(joint_names_, gh.getGoal()->trajectory.joint_names);
std::vector<unsigned int> permutation_vector = permutation(gh.getGoal()->trajectory.joint_names, joint_names_);

if (permutation_vector.empty())
{
Expand Down Expand Up @@ -624,19 +630,28 @@ queryStateService(control_msgs::QueryTrajectoryState::Request& req,
curr_trajectory_box_.get(curr_traj_ptr);
Trajectory& curr_traj = *curr_traj_ptr;

typename Segment::State state;
typename Trajectory::const_iterator segment_it = sample(curr_traj, sample_time.toSec(), state);
if (curr_traj.end() == segment_it)
typename Segment::State response_point = typename Segment::State(joint_names_.size());

for (unsigned int i = 0; i < joints_.size(); ++i)
{
ROS_ERROR_STREAM_NAMED(name_, "Requested sample time preceeds trajectory start time.");
return false;
}
typename Segment::State state;
typename TrajectoryPerJoint::const_iterator segment_it = sample(curr_traj[i], sample_time.toSec(), state);
if (curr_traj[i].end() == segment_it)
{
ROS_ERROR_STREAM_NAMED(name_, "Requested sample time precedes trajectory start time.");
return false;
}

response_point.position[i] = state.position[0];
response_point.velocity[i] = state.velocity[0];
response_point.acceleration[i] = state.acceleration[0];
}

// Populate response
resp.name = joint_names_;
resp.position = state.position;
resp.velocity = state.velocity;
resp.acceleration = state.acceleration;
resp.position = response_point.position;
resp.velocity = response_point.velocity;
resp.acceleration = response_point.acceleration;

return true;
}
Expand Down Expand Up @@ -676,7 +691,11 @@ setHoldPosition(const ros::Time& time)
// - Create segment that goes from current state to above zero velocity state, in the desired time
// NOTE: The symmetry assumption from the second point above might not hold for all possible segment types

assert(1 == hold_trajectory_ptr_->size());
assert(joint_names_.size() == hold_trajectory_ptr_->size());

ROS_INFO_STREAM_NAMED(name_, "Setting hold position");
typename Segment::State hold_start_state_ = typename Segment::State(1);
typename Segment::State hold_end_state_ = typename Segment::State(1);

const typename Segment::Time start_time = time.toSec();
const typename Segment::Time end_time = time.toSec() + stop_trajectory_duration_;
Expand All @@ -686,24 +705,24 @@ setHoldPosition(const ros::Time& time)
const unsigned int n_joints = joints_.size();
for (unsigned int i = 0; i < n_joints; ++i)
{
hold_start_state_.position[i] = joints_[i].getPosition();
hold_start_state_.velocity[i] = joints_[i].getVelocity();
hold_start_state_.acceleration[i] = 0.0;
hold_start_state_.position[0] = joints_[i].getPosition();
hold_start_state_.velocity[0] = joints_[i].getVelocity();
hold_start_state_.acceleration[0] = 0.0;

hold_end_state_.position[i] = joints_[i].getPosition();
hold_end_state_.velocity[i] = -joints_[i].getVelocity();
hold_end_state_.acceleration[i] = 0.0;
}
hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
end_time_2x, hold_end_state_);
hold_end_state_.position[0] = joints_[i].getPosition();
hold_end_state_.velocity[0] = -joints_[i].getVelocity();
hold_end_state_.acceleration[0] = 0.0;

// Sample segment at its midpoint, that should have zero velocity
hold_trajectory_ptr_->front().sample(end_time, hold_end_state_);
(*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
end_time_2x, hold_end_state_);

// Now create segment that goes from current state to one with zero end velocity
hold_trajectory_ptr_->front().init(start_time, hold_start_state_,
end_time, hold_end_state_);
// Sample segment at its midpoint, that should have zero velocity
(*hold_trajectory_ptr_)[i].front().sample(end_time, hold_end_state_);

// Now create segment that goes from current state to one with zero end velocity
(*hold_trajectory_ptr_)[i].front().init(start_time, hold_start_state_,
end_time, hold_end_state_);
}
curr_trajectory_box_.set(hold_trajectory_ptr_);
}

Expand Down
Loading