Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Merge pull request #31 from hemes/master

Browse Source 
Added check to ensure robot pose matches initial trajectory point.
This commit is contained in:
Thomas Timm Andersen
2016-01-14 15:47:33 +01:00
parent a3e174626c f104b50026
commit d1905d6745
1 changed files with 22 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/ur_ros_wrapper.cpp
View File

@@ -298,7 +298,7 @@ private:
print_error(result_.error_string); print_error(result_.error_string);
return; return;
} }
if (!has_velocities()) { if (!has_velocities()) {
result_.error_code = result_.INVALID_GOAL; result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without velocities"; result_.error_string = "Received a goal without velocities";
@@ -326,6 +326,14 @@ private:
} }
reorder_traj_joints(goal.trajectory); reorder_traj_joints(goal.trajectory);
if (!start_positions_match(goal.trajectory, 0.01)) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Goal start doesn't match current pose";
gh.setRejected(result_, result_.error_string);
print_error(result_.error_string);
return;
}
std::vector<double> timestamps; std::vector<double> timestamps;
std::vector<std::vector<double> > positions, velocities; std::vector<std::vector<double> > positions, velocities;
@@ -476,6 +484,19 @@ private:
return true; return true;
} }
bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps)
{
for (unsigned int i = 0; i < traj.points[0].positions.size(); i++)
{
std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
if( fabs(traj.points[0].positions[i] - qActual[i]) > eps )
{
return false;
}
}
return true;
}
bool has_limited_velocities() { bool has_limited_velocities() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal(); *goal_handle_.getGoal();