joint_spline_trajectory_controller.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
37 #ifndef JOINT_TRAJECTORY_CONTROLLER_H__
38 #define JOINT_TRAJECTORY_CONTROLLER_H__
39 
40 #include <vector>
41 
42 #include <boost/scoped_ptr.hpp>
43 #include <boost/shared_ptr.hpp>
44 #include <ros/node_handle.h>
45 #include <control_toolbox/pid.h>
49 
50 #include "trajectory_msgs/JointTrajectory.h"
51 //#include "robot_mechanism_controllers/Trajectory.h"
52 #include "pr2_controllers_msgs/QueryTrajectoryState.h"
53 #include "pr2_controllers_msgs/JointTrajectoryControllerState.h"
54 
55 namespace controller {
56 
58 {
59 public:
60 
63 
65 
66  void starting();
67  void update();
68 
69 private:
73  std::vector<pr2_mechanism_model::JointState*> joints_;
74  std::vector<control_toolbox::Pid> pids_;
75 
77 
78  void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
80 
81  bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req,
82  pr2_controllers_msgs::QueryTrajectoryState::Response &resp);
84 
85  boost::scoped_ptr<
87  pr2_controllers_msgs::JointTrajectoryControllerState> > controller_state_publisher_;
88 
89 
90  // ------ Mechanisms for passing the trajectory into realtime
91 
92  // coef[0] + coef[1]*t + ... + coef[5]*t^5
93  struct Spline
94  {
95  std::vector<double> coef;
96 
97  Spline() : coef(6, 0.0) {}
98  };
99 
100  struct Segment
101  {
102  double start_time;
103  double duration;
104  std::vector<Spline> splines;
105  };
106  typedef std::vector<Segment> SpecifiedTrajectory;
107 
110 
111  // Holds the trajectory that we are currently following. The mutex
112  // guarding current_trajectory_ is locked from within realtime, so
113  // it may only be locked for a bounded duration.
114  //boost::shared_ptr<const SpecifiedTrajectory> current_trajectory_;
115  //boost::recursive_mutex current_trajectory_lock_RT_;
116 
117  std::vector<double> q, qd, qdd; // Preallocated in init
118 
119  // Samples, but handling time bounds. When the time is past the end
120  // of the spline duration, the position is the last valid position,
121  // and the derivatives are all 0.
122  static void sampleSplineWithTimeBounds(const std::vector<double>& coefficients, double duration, double time,
123  double& position, double& velocity, double& acceleration);
124 };
125 
126 }
127 
128 #endif
controller::JointSplineTrajectoryController::Segment::start_time
double start_time
Definition: joint_spline_trajectory_controller.h:102
realtime_publisher.h
node_handle.h
controller::JointSplineTrajectoryController::commandCB
void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
Definition: joint_spline_trajectory_controller.cpp:344
boost::shared_ptr
controller::JointSplineTrajectoryController::loop_count_
int loop_count_
Definition: joint_spline_trajectory_controller.h:70
controller::JointSplineTrajectoryController::SpecifiedTrajectory
std::vector< Segment > SpecifiedTrajectory
Definition: joint_spline_trajectory_controller.h:106
controller::JointSplineTrajectoryController::current_trajectory_box_
realtime_tools::RealtimeBox< boost::shared_ptr< const SpecifiedTrajectory > > current_trajectory_box_
Definition: joint_spline_trajectory_controller.h:109
controller::JointSplineTrajectoryController::qdd
std::vector< double > qdd
Definition: joint_spline_trajectory_controller.h:117
controller::JointSplineTrajectoryController::Segment
Definition: joint_spline_trajectory_controller.h:100
controller::JointSplineTrajectoryController::starting
void starting()
Definition: joint_spline_trajectory_controller.cpp:249
controller::JointSplineTrajectoryController::init
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
Definition: joint_spline_trajectory_controller.cpp:155
ros::ServiceServer
controller::JointSplineTrajectoryController
Definition: joint_spline_trajectory_controller.h:57
realtime_tools::RealtimePublisher
controller::JointSplineTrajectoryController::serve_query_state_
ros::ServiceServer serve_query_state_
Definition: joint_spline_trajectory_controller.h:83
controller::JointSplineTrajectoryController::Spline
Definition: joint_spline_trajectory_controller.h:93
controller::JointSplineTrajectoryController::~JointSplineTrajectoryController
~JointSplineTrajectoryController()
Definition: joint_spline_trajectory_controller.cpp:149
controller::JointSplineTrajectoryController::last_time_
ros::Time last_time_
Definition: joint_spline_trajectory_controller.h:72
controller.h
controller
pr2_mechanism_model::RobotState
controller::JointSplineTrajectoryController::Spline::coef
std::vector< double > coef
Definition: joint_spline_trajectory_controller.h:95
controller::JointSplineTrajectoryController::qd
std::vector< double > qd
Definition: joint_spline_trajectory_controller.h:117
realtime_tools::RealtimeBox
controller::JointSplineTrajectoryController::update
void update()
Definition: joint_spline_trajectory_controller.cpp:268
controller::JointSplineTrajectoryController::q
std::vector< double > q
Definition: joint_spline_trajectory_controller.h:117
controller::JointSplineTrajectoryController::pids_
std::vector< control_toolbox::Pid > pids_
Definition: joint_spline_trajectory_controller.h:74
controller::JointSplineTrajectoryController::controller_state_publisher_
boost::scoped_ptr< realtime_tools::RealtimePublisher< pr2_controllers_msgs::JointTrajectoryControllerState > > controller_state_publisher_
Definition: joint_spline_trajectory_controller.h:87
controller::JointSplineTrajectoryController::sub_command_
ros::Subscriber sub_command_
Definition: joint_spline_trajectory_controller.h:79
controller::JointSplineTrajectoryController::Segment::splines
std::vector< Spline > splines
Definition: joint_spline_trajectory_controller.h:104
pr2_controller_interface::Controller
controller::JointSplineTrajectoryController::sampleSplineWithTimeBounds
static void sampleSplineWithTimeBounds(const std::vector< double > &coefficients, double duration, double time, double &position, double &velocity, double &acceleration)
Definition: joint_spline_trajectory_controller.cpp:628
ros::Time
controller::JointSplineTrajectoryController::node_
ros::NodeHandle node_
Definition: joint_spline_trajectory_controller.h:76
controller::JointSplineTrajectoryController::JointSplineTrajectoryController
JointSplineTrajectoryController()
Definition: joint_spline_trajectory_controller.cpp:144
controller::JointSplineTrajectoryController::joints_
std::vector< pr2_mechanism_model::JointState * > joints_
Definition: joint_spline_trajectory_controller.h:73
controller::JointSplineTrajectoryController::queryStateService
bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req, pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
Definition: joint_spline_trajectory_controller.cpp:585
pid.h
controller::JointSplineTrajectoryController::Segment::duration
double duration
Definition: joint_spline_trajectory_controller.h:103
controller::JointSplineTrajectoryController::robot_
pr2_mechanism_model::RobotState * robot_
Definition: joint_spline_trajectory_controller.h:71
controller::JointSplineTrajectoryController::Spline::Spline
Spline()
Definition: joint_spline_trajectory_controller.h:97
ros::NodeHandle
ros::Subscriber
realtime_box.h


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Sat Nov 12 2022 03:33:22