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

Added check to ensure robot pose matches initial trajectory point.

Browse Source
This commit is contained in:
hemes
2016-01-13 15:04:08 -06:00
parent a3e174626c
commit f104b50026
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);
return;
}
if (!has_velocities()) {
result_.error_code = result_.INVALID_GOAL;
result_.error_string = "Received a goal without velocities";
@@ -326,6 +326,14 @@ private:
}
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<std::vector<double> > positions, velocities;
@@ -476,6 +484,19 @@ private:
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() {
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
*goal_handle_.getGoal();