00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <pr2_mechanism_controllers/base_kinematics.h>
00039 #include <kdl/tree.hpp>
00040
00041 using namespace controller;
00042
00043
00044 bool Wheel::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name)
00045 {
00046 wheel_stuck_ = 0;
00047 direction_multiplier_ = 1;
00048 wheel_speed_filtered_ = 0;
00049 wheel_speed_error_ = 0;
00050 wheel_speed_cmd_ = 0;
00051 wheel_speed_actual_ = 0;
00052
00053 boost::shared_ptr<const urdf::Link> link;
00054 link = robot_state->model_->robot_model_.getLink(link_name);
00055 if (!link){
00056 ROS_ERROR("Could not find link with name %s",link_name.c_str());
00057 return false;
00058 }
00059
00060 if(!link->collision)
00061 {
00062 ROS_ERROR("Link %s does not have collision description. Add collision description for link to urdf.",link_name.c_str());
00063 return false;
00064 }
00065 if(!link->collision->geometry)
00066 {
00067 ROS_ERROR("Link %s does not have collision geometry description. Add collision geometry description for link to urdf.",link_name.c_str());
00068 return false;
00069 }
00070 if(link->collision->geometry->type != urdf::Geometry::CYLINDER)
00071 {
00072 ROS_ERROR("Link %s does not have cylinder geometry",link_name.c_str());
00073 return false;
00074 }
00075 wheel_radius_ = (dynamic_cast<urdf::Cylinder*>(link->collision->geometry.get()))->radius;
00076 ROS_DEBUG("wheel name: %s, radius: %f",link_name.c_str(),wheel_radius_);
00077 link_name_ = link_name;
00078 joint_name_ = link->parent_joint->name;
00079
00080 joint_ = robot_state->getJointState(joint_name_);
00081 if (!joint_){
00082 ROS_ERROR("Could not find joint with name %s",joint_name_.c_str());
00083 return false;
00084 }
00085
00086 urdf::Vector3 offset = link->parent_joint->parent_to_joint_origin_transform.position;
00087 offset_.x = offset.x;
00088 offset_.y = offset.y;
00089 offset_.z = offset.z;
00090
00091 ROS_DEBUG("Loading wheel: %s",link_name_.c_str());
00092 ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
00093 return true;
00094 }
00095
00096 bool Caster::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node, std::string link_name)
00097 {
00098 caster_stuck_ = 0;
00099 caster_speed_ = 0;
00100 caster_speed_filtered_ = 0;
00101 caster_speed_error_ = 0;
00102 caster_position_error_ = 0;
00103 steer_angle_stored_ = 0;
00104 steer_velocity_desired_ = 0;
00105 steer_angle_actual_ = 0;
00106 num_children_ = 0;
00107
00108 boost::shared_ptr<const urdf::Link> link;
00109 link = robot_state->model_->robot_model_.getLink(link_name);
00110 if (!link){
00111 ROS_ERROR("Could not find link with name %s",link_name.c_str());
00112 return false;
00113 }
00114 ROS_DEBUG("caster name: %s",link_name.c_str());
00115 link_name_ = link_name;
00116 joint_name_ = link->parent_joint->name;
00117
00118 joint_ = robot_state->getJointState(joint_name_);
00119 if (!joint_){
00120 ROS_ERROR("Could not find joint with name %s",joint_name_.c_str());
00121 return false;
00122 }
00123 urdf::Vector3 offset = link->parent_joint->parent_to_joint_origin_transform.position;
00124 offset_.x = offset.x;
00125 offset_.y = offset.y;
00126 offset_.z = offset.z;
00127
00128 for(unsigned int i=0; i < link->child_links.size(); i++)
00129 {
00130 boost::shared_ptr<urdf::Link> child = link->child_links[i];
00131 Wheel tmp;
00132 parent_->wheel_.push_back(tmp);
00133 if(!parent_->wheel_[parent_->num_wheels_].init(robot_state, node, child->name))
00134 {
00135 ROS_ERROR("Could not initialize caster %s",link_name.c_str());
00136 return false;
00137 }
00138 parent_->num_wheels_++;
00139 num_children_++;
00140 }
00141 return true;
00142 }
00143
00144 bool BaseKinematics::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &node)
00145 {
00146 std::string caster_names_string;
00147 std::vector<std::string> caster_names;
00148 name_ = node.getNamespace();
00149
00150 MAX_DT_ = 0.01;
00151 num_wheels_ = 0;
00152 num_casters_ = 0;
00153
00154 robot_state_ = robot_state;
00155
00156 node.param<std::string> ("caster_names",caster_names_string,"");
00157 std::stringstream ss(caster_names_string);
00158 std::string tmp;
00159 while(ss >> tmp)
00160 {
00161 caster_names.push_back(tmp);
00162 }
00163
00164 for(unsigned int i=0; i < caster_names.size(); i++)
00165 {
00166 Caster tmp;
00167 caster_.push_back(tmp);
00168 caster_[num_casters_].parent_ = this;
00169 ROS_DEBUG("caster name: %s",caster_names[i].c_str());
00170 ros::NodeHandle n(name_);
00171 if(!caster_[num_casters_].init(robot_state, n, caster_names[i]))
00172 {
00173 ROS_ERROR("Could not initialize base kinematics");
00174 return false;
00175 }
00176 num_casters_++;
00177 }
00178
00179
00180 double multiplier;
00181 node.param<double> ("wheel_radius_multiplier", multiplier, 1.0);
00182 int wheel_counter = 0;
00183 for(int j = 0; j < num_casters_; j++)
00184 {
00185 for(int i = 0; i < caster_[j].num_children_; i++)
00186 {
00187 wheel_[wheel_counter].parent_ = &(caster_[j]);
00188 wheel_[wheel_counter].wheel_radius_*=multiplier;
00189 wheel_counter++;
00190 }
00191 }
00192 return true;
00193 }
00194
00195 void Wheel::updatePosition()
00196 {
00197 geometry_msgs::Point result = parent_->offset_;
00198 double costh = cos(parent_->joint_->position_);
00199 double sinth = sin(parent_->joint_->position_);
00200 result.x += costh * offset_.x - sinth * offset_.y;
00201 result.y += sinth * offset_.x + costh * offset_.y;
00202 result.z = 0.0;
00203 position_ = result;
00204 }
00205
00206 void BaseKinematics::computeWheelPositions()
00207 {
00208 for(int i = 0; i < num_wheels_; i++)
00209 {
00210 wheel_[i].updatePosition();
00211 }
00212
00213 }
00214
00215 geometry_msgs::Twist BaseKinematics::pointVel2D(const geometry_msgs::Point& pos, const geometry_msgs::Twist& vel)
00216 {
00217 geometry_msgs::Twist result;
00218 result.linear.x = vel.linear.x - pos.y * vel.angular.z;
00219 result.linear.y = vel.linear.y + pos.x * vel.angular.z;
00220 result.angular.z = 0;
00221 return result;
00222 }
