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

Hold joint position if tolerances were violated #395

Open
wants to merge 3 commits into
base: kinetic-devel
Choose a base branch
from
Open

Hold joint position if tolerances were violated #395

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f19e7a4
Holding the current position if path tolerances were violated
Martin-Oehler Dec 11, 2018
b7b23f5
holding position when goal tolerance was violated
Martin-Oehler Dec 13, 2018
3d7f134
extended test cases for tolerance violations to check for movement af…
Martin-Oehler Jan 11, 2019
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
7 changes: 7 additions & 0 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 @@ -417,6 +417,10 @@ update(const ros::Time& time, const ros::Duration& period)
ROS_ERROR_STREAM_NAMED(name_,"Path tolerances failed for joint: " << joint_names_[i]);
checkStateTolerancePerJoint(state_joint_error_, joint_tolerances.state_tolerance, true);
}

// If we violate path tolerance then hold current position
setHoldPosition(time_data.uptime, rt_active_goal_);

rt_segment_goal->preallocated_result_->error_code =
control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_);
Expand Down Expand Up @@ -453,6 +457,9 @@ update(const ros::Time& time, const ros::Duration& period)
checkStateTolerancePerJoint(state_joint_error_, tolerances.goal_state_tolerance, true);
}

// If we violate goal tolerances then hold current position
setHoldPosition(time_data.uptime, rt_active_goal_);

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();
Expand Down
23 changes: 22 additions & 1 deletion joint_trajectory_controller/test/joint_trajectory_controller_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1244,6 +1244,17 @@ TEST_F(JointTrajectoryControllerTest, pathToleranceViolation)
EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout));
EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED);

// Check that we're not moving
StateConstPtr state1 = getState();
ros::Duration(0.5).sleep(); // Wait
Copy link
Contributor

Choose a reason for hiding this comment

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

This might be a little bit too high for the hold trajectory to be effective, but with the position plausibility checks this does not matter (apart from longer test time..)

Copy link
Author

Choose a reason for hiding this comment

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

I made this consistent with the other tests. See my other reply.

Copy link
Contributor

Choose a reason for hiding this comment

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

As long as system time is used, it is difficult to check that the robot stopped in time.
Because there can be delay in the state-publisher.

StateConstPtr state2 = getState();
for (unsigned int i = 0; i < n_joints; ++i)
{
EXPECT_NEAR(state1->desired.positions[i], state2->desired.positions[i], EPS);
Copy link
Contributor

Choose a reason for hiding this comment

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

According to the failure log, the velocity in state1 is non-zero, but in state2 it is zero.
This shows that your path is working!

I would make the test check state1 velocity and acceleration to be non-zero (=moving) and the ones in state2 should be zero. In addition the position change between state1 and state2 should be in the range of the largest possible step in the hold-trajectory.

Copy link
Author

Choose a reason for hiding this comment

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

There are a few other test cases that check for movement, for example here. I made my check similar for consistency.

Copy link
Contributor

Choose a reason for hiding this comment

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

These tests don't have the delay enabled and a big delay before acquiring the states.

Copy link
Author

Choose a reason for hiding this comment

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

Okay, I will update the test with your proposal.

Copy link
Author

Choose a reason for hiding this comment

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

I would make the test check state1 velocity and acceleration to be non-zero (=moving) and the ones in state2 should be zero.

state1 velocity and acceleration are only non-zero, if stop_trajectory_duration != 0. I guess, I have to test for multiple cases here.

In addition the position change between state1 and state2 should be in the range of the largest possible step in the hold-trajectory.

Do you know how to find this value? The actual implementation samples a trajectory.

Copy link
Contributor

Choose a reason for hiding this comment

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

I would make the test check state1 velocity and acceleration to be non-zero (=moving) and the ones in state2 should be zero. In addition the position change between state1 and state2 should be in the range of the largest possible step in the hold-trajectory.

Sounds good. I think we should strive for an assertion that reliably checks, if a hold trajectory is executed as expected. This could then be used in all relevant test cases. However, I would separate it from this PR.

EXPECT_NEAR(state1->desired.velocities[i], state2->desired.velocities[i], EPS);
EXPECT_NEAR(state1->desired.accelerations[i], state2->desired.accelerations[i], EPS);
}

// Restore perfect control
{
std_msgs::Float64 smoothing;
Expand All @@ -1266,7 +1277,7 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation)
// Make robot respond with a delay
{
std_msgs::Float64 smoothing;
smoothing.data = 0.95;
smoothing.data = 0.98;
Copy link
Contributor

Choose a reason for hiding this comment

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

Why did you chnage this?

Copy link
Author

Choose a reason for hiding this comment

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

If I do not make the movement slower, the joints are too close to the goal position and the position check always passes.

Copy link
Contributor

Choose a reason for hiding this comment

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

This makes the movement faster.

Copy link
Author

Choose a reason for hiding this comment

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

A smoothing value of 0 corresponds to direct control whereas a value of 1 makes the robot not respond at all. Therefore, we movement is delayed more. "Slower" was the wrong term, I guess.

Copy link
Contributor

Choose a reason for hiding this comment

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

Ah, you are right, my bad!

smoothing_pub.publish(smoothing);
ros::Duration(0.5).sleep();
}
Expand All @@ -1289,13 +1300,23 @@ TEST_F(JointTrajectoryControllerTest, goalToleranceViolation)
EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout));
EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED);

// Check that we're not moving after restoring perfect control
StateConstPtr state1 = getState();

// Restore perfect control
{
std_msgs::Float64 smoothing;
smoothing.data = 0.0;
smoothing_pub.publish(smoothing);
ros::Duration(0.5).sleep();
}

StateConstPtr state2 = getState();
for (unsigned int i = 0; i < n_joints; ++i)
{
EXPECT_NEAR(state1->actual.positions[i], state2->actual.positions[i], EPS);
Copy link
Contributor

Choose a reason for hiding this comment

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

You check it the same way as in the other test.
To rule out other effect, just use the desired values as well.

Copy link
Author

Choose a reason for hiding this comment

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

In this case, taking the desired values is not possible. The trajectory has already finished when the goal tolerance is checked. Therefore, desired values do not change anymore.

Copy link
Contributor

Choose a reason for hiding this comment

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

This test should check the behavior of the controller (=desired value). And according to the test output, this code samples the hold trajectory properly.

Copy link
Author

Choose a reason for hiding this comment

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

The desired position should match the actual position at the time of the goal tolerance violation. I could rewrite the test to check that.

Copy link
Contributor

Choose a reason for hiding this comment

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

The desired position should match the actual position at the time of the goal tolerance violation

The final position is based on the hold trajectory -> will move a little bit to obey the speed limits etc.

Copy link
Author

Choose a reason for hiding this comment

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

I am not sure, if I understand you correctly. Before my fix, the desired velocity and acceleration of state1 would be 0, because that is the final state of the trajectory. After my fix and with stop_trajectory_duration == 0 vel and acc of state1 would also be zero, because that is what they are set to.

Copy link
Contributor

Choose a reason for hiding this comment

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

Before your fix, the desired velocity and acceleration of state1 would ne non-zero, because the controller is still moving towards the goal and motion does not stop.
Your fix make it stop.

The tests must command a goal that takes much longer to reach than the hold trajectory (to stop the motion).

Copy link
Author

Choose a reason for hiding this comment

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

I don't think, we are talking about the same problem here. I try to summarize the scenario to clarify the situation:
The robot is moving towards some goal but is delayed and therefore does not follow the trajectory very closely. Now the desired trajectory is finished (desired velocity/acceleration are zero) and goal tolerances are checked. The robot is still moving as it is delayed.
Previous to my PR the robot would keep moving and reach the goal at an unknown time. My PR changes the behavior so the robot stops moving towards the original goal (as moving there would be useless/dangerous now) and holds its position.

I think what should be tested here, is that the robot stays close to its current position after the goal tolerance violation occurred. I agree, that the desired trajectory should be checked, but simply checking velocity/acceleration is not an option as the original behavior also commands a zero velocity/acceleration in its final state, as the trajectory has finished.

Copy link
Contributor

Choose a reason for hiding this comment

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

The tests must command a goal that takes much longer to reach than the hold trajectory (to stop the motion).

I thought the same. This could be done in the path-tolerance test case.

I suggest to synchronize with #457 in order to avoid doubling the effort.

@Martin-Oehler Are you still interested in continuing on this?

Copy link
Author

Choose a reason for hiding this comment

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

Unfortunately, I am currently occupied by other projects and won't have time to further look into this. Feel free to take over if you are interested.

EXPECT_NEAR(state1->actual.velocities[i], state2->actual.velocities[i], EPS);
}
}

int main(int argc, char** argv)
Expand Down