35 has_active_goal_(false), trajectory_finished_(true)
43 joint_names_.push_back((std::string)
"katana_r_finger_joint");
44 joint_names_.push_back((std::string)
"katana_l_finger_joint");
51 std::string ns = std::string(
"constraints/") +
joint_names_[i];
53 pn.
param(ns +
"/goal", g, -1.0);
54 pn.
param(ns +
"/trajectory", t, -1.0);
68 "katana gripper joint trajectory action server started on topic katana_arm_controller/gripper_joint_trajectory_action");
78 const std::vector<std::string> &b)
80 if (a.size() != b.size())
83 for (
size_t i = 0; i < a.size(); ++i)
85 if (count(b.begin(), b.end(), a[i]) != 1)
88 for (
size_t i = 0; i < b.size(); ++i)
90 if (count(a.begin(), a.end(), b[i]) != 1)
114 bool inside_goal_constraints =
false;
121 inside_goal_constraints =
true;
126 if (inside_goal_constraints)
136 ROS_DEBUG(
"Still have some time left to make it.");
155 ROS_DEBUG(
"current_angle_: %f desired_angle_: %f", current_angle_, desired_angle_);
165 ROS_DEBUG(
"KatanaGripperJointTrajectoryController::goalCB");
170 ROS_ERROR(
"KatanaGripperJointTrajectoryController::goalCB: Joints on incoming goal don't match our joints");
175 double desired_start_pos = gh.
getGoal()->trajectory.points[0].positions[0];
177 ROS_ERROR(
"Input trajectory is invalid (difference between desired and current point too high: %f). This might crash Gazebo with error \"The minimum corner of the box must be less than or equal to maximum corner\".", fabs(desired_start_pos -
current_point_.
position));
217 if (traj.points.empty())
219 ROS_WARN(
"KatanaGripperJointTrajectoryController::setCurrentTrajectory: Empty trajectory");
248 if (time.
toSec() < traj.header.stamp.toSec())
260 size_t traj_segment = 0;
261 bool found_traj_seg =
false;
262 size_t numof_points = traj.points.size();
263 for (
size_t i = 1; i < numof_points; i++)
265 if (traj.points[i].time_from_start >= relative_time)
268 found_traj_seg =
true;
278 "Trajectory finished (requested time %f time_from_start[last_point]: %f)", relative_time.
toSec(), traj.points[traj.points.size()-1].time_from_start.toSec());
288 size_t start_index = traj_segment - 1;
289 size_t end_index = traj_segment;
291 double start_pos = traj.points[start_index].positions[0];
292 double start_vel = traj.points[start_index].velocities[0];
297 double end_pos = traj.points[end_index].positions[0];
298 double end_vel = traj.points[end_index].velocities[0];
303 double time_from_start = traj.points[end_index].time_from_start.toSec();
309 std::vector<double> coefficients;
313 double sample_pos = 0, sample_vel = 0, sample_acc = 0;
320 GRKAPoint point = {sample_pos, sample_vel};
void setCurrentTrajectory(trajectory_msgs::JointTrajectory traj)
double stopped_velocity_tolerance_
std::vector< std::string > joint_names_
trajectory_msgs::JointTrajectory current_traj_
std::map< std::string, double > goal_constraints_
static const double GRIPPER_ANGLE_THRESHOLD
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
boost::shared_ptr< const Goal > getGoal() const
actionlib::ActionServer< control_msgs::JointTrajectoryAction > JTAS
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
virtual ~KatanaGripperJointTrajectoryController()
void setAccepted(const std::string &text=std::string(""))
bool isTrajectoryFinished()
bool trajectory_finished_
void getCubicSplineCoefficients(double start_pos, double start_vel, double end_pos, double end_vel, double time, std::vector< double > &coefficients)
Calculates cubic spline coefficients given the start and end way-points.
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void goalCB(GoalHandle gh)
void sampleCubicSpline(const std::vector< double > &coefficients, double time, double &position, double &velocity, double &acceleration)
Samples a cubic spline segment at a particular time.
void cancelCB(GoalHandle gh)
GRKAPoint getNextDesiredPoint(ros::Time time)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
KatanaGripperJointTrajectoryController(ros::NodeHandle pn)
void getGains(double &p, double &i, double &d, double &i_max, double &i_min)
std::map< std::string, double > trajectory_constraints_
GRKAPoint last_desired_point_
bool currentIsDesiredAngle()
static bool setsEqual(const std::vector< std::string > &a, const std::vector< std::string > &b)
double goal_time_constraint_