base_kinematics.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 /*
35  * Author: Sachin Chitta and Matthew Piccoli
36  */
37 
38 #ifndef PR2_BASE_KINEMATICS_H
39 #define PR2_BASE_KINEMATICS_H
40 
43 #include <geometry_msgs/Twist.h>
44 #include <geometry_msgs/Point.h>
46 #include <boost/thread/condition.hpp>
47 
48 namespace controller
49 {
50  class Wheel;
51  class Caster;
52  class BaseKinematics;
56  class Wheel
57  {
58  public:
59 
64 
68  geometry_msgs::Point offset_;
69 
73  std::string joint_name_;
74 
78  std::string link_name_;
79 
80 
84  geometry_msgs::Point position_;
85 
90 
95 
100 
105 
110 
115 
120 
125 
131  bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name);
132 
136  void updatePosition();
137  };
138 
142  class Caster
143  {
144  public:
145 
150 
154  geometry_msgs::Point offset_;
155 
159  std::string link_name_;
160 
164  std::string joint_name_;
165 
166  //geometry_msgs::Point position_;
167 
172 
177 
182 
187 
192 
197 
202 
207 
212 
217 
222 
223  bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name);
224  };
225 
230  {
231  public:
232 
239  bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node);
240 
244  void computeWheelPositions();
245 
252  geometry_msgs::Twist pointVel2D(const geometry_msgs::Point& pos, const geometry_msgs::Twist& vel);
253 
258 
263 
268 
272  std::vector<Wheel> wheel_;
273 
277  std::vector<Caster> caster_;
278 
282  std::string xml_caster_name_;
283 
287  std::string xml_wheel_name_;
288 
292  std::string name_;
293 
297  double MAX_DT_;
298 
302  std::string robot_base_id_;
303  };
304 } // namespace
305 #endif
double wheel_speed_cmd_
desired wheel speed
pr2_mechanism_model::JointState * joint_
JointState for this caster joint.
std::string link_name_
name of the link
double wheel_radius_
wheel radius scale (based on the default wheel radius in Basekinematics)
double caster_speed_
remembers the caster&#39;s current speed
std::string joint_name_
name of the joint
geometry_msgs::Point offset_
offset from the center of the base
std::string xml_caster_name_
the name of the casters in the xml file
double caster_speed_filtered_
caster speed filtered with alpha
std::vector< Caster > caster_
vector of every caster attached to the base
int caster_stuck_
remembers if the caster is stalled
bool init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name)
Loads wheel&#39;s information from the xml description file and param server.
std::string joint_name_
name of the joint
double steer_velocity_desired_
vector of desired caster steer speeds
geometry_msgs::Point offset_
default offset from the parent caster before any transformations
double caster_position_error_
difference between desired and actual angles of the casters
double MAX_DT_
maximum dT used in computation of interpolated velocity command
geometry_msgs::Point position_
offset_ after rotation transformation from the parent caster&#39;s position
int wheel_stuck_
remembers if the wheel is stalled
int num_wheels_
number of wheels connected to the base
std::vector< Wheel > wheel_
vector of every wheel attached to the base
Caster * parent_
the caster on which this wheel is mounted
double wheel_speed_actual_
actual wheel speeds
int direction_multiplier_
specifies the default direction of the wheel
BaseKinematics * parent_
BaseKinematics to which this caster belongs.
pr2_mechanism_model::JointState * joint_
JointState for this wheel joint.
int num_children_
the number of child wheels that are attached to this caster
std::string xml_wheel_name_
the name of the wheels in the xml file
double steer_angle_actual_
actual caster steer angles
double caster_speed_error_
difference between desired and actual speeds of the casters
double wheel_speed_filtered_
wheel speed filtered with alpha
std::string name_
name of this BaseKinematics (generally associated with whatever controller is using it) ...
double steer_angle_desired_
actual caster steer angles
std::string robot_base_id_
Name(string id) of the robot base frame.
double steer_angle_stored_
stored caster steer angles
pr2_mechanism_model::RobotState * robot_state_
remembers everything about the state of the robot
int num_casters_
number of casters connected to the base
double wheel_speed_error_
difference between desired and actual speed
std::string link_name_
name of the link
void updatePosition()
Computes 2d postion of the wheel relative to the center of the base.


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Mon Jun 10 2019 14:26:33