28 #ifndef KATANA_GRIPPER_JOINT_TRAJECTORY_CONTROLLER_H_ 29 #define KATANA_GRIPPER_JOINT_TRAJECTORY_CONTROLLER_H_ 34 #include <trajectory_msgs/JointTrajectory.h> 35 #include <control_msgs/JointTrajectoryAction.h> 36 #include <control_msgs/JointTrajectoryControllerState.h> 83 void goalCB(GoalHandle gh);
87 static bool setsEqual(
const std::vector<std::string> &a,
const std::vector<std::string> &b);
97 void getGains(
double &p,
double &i,
double &d,
double &i_max,
double &i_min);
101 this->current_point_ = point;
130 for (
int i=1; i<=n; i++)
132 powers[i] = powers[i-1]*x;
147 double end_pos,
double end_vel,
double time, std::vector<double>& coefficients);
153 double& position,
double& velocity,
double& acceleration);
157 double end_pos,
double end_vel,
double time, std::vector<double>& coefficients)
159 coefficients.resize(4);
164 coefficients[0] = start_pos;
165 coefficients[1] = start_vel;
166 coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
167 coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
171 double& position,
double& velocity,
double& acceleration)
176 position = t[0]*coefficients[0] +
177 t[1]*coefficients[1] +
178 t[2]*coefficients[2] +
179 t[3]*coefficients[3];
181 velocity = t[0]*coefficients[1] +
182 2.0*t[1]*coefficients[2] +
183 3.0*t[2]*coefficients[3];
185 acceleration = 2.0*t[0]*coefficients[2] +
186 6.0*t[1]*coefficients[3];
void setCurrentTrajectory(trajectory_msgs::JointTrajectory traj)
double stopped_velocity_tolerance_
bool hasActiveGoal() const
std::vector< std::string > joint_names_
trajectory_msgs::JointTrajectory current_traj_
std::map< std::string, double > goal_constraints_
static void generatePowers(int n, double x, double *powers)
static const double GRIPPER_ANGLE_THRESHOLD
actionlib::ActionServer< control_msgs::JointTrajectoryAction > JTAS
virtual ~KatanaGripperJointTrajectoryController()
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 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)
KatanaGripperJointTrajectoryController(ros::NodeHandle pn)
JTAS::GoalHandle GoalHandle
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_
void setCurrentPoint(GRKAPoint point)