cartesian_pose_controller.cpp
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 
30 /*
31  * Author: Wim Meeussen
32  */
33 
34 
35 
37 #include <algorithm>
38 #include "kdl/chainfksolverpos_recursive.hpp"
40 #include "tf_conversions/tf_kdl.h"
41 
42 
43 using namespace KDL;
44 using namespace tf;
45 using namespace std;
46 
48 
49 namespace controller {
50 
51 CartesianPoseController::CartesianPoseController()
52 : robot_state_(NULL)
53 {}
54 
56 {
57  command_filter_.reset();
58 }
59 
60 
62 {
63  node_ = n;
64 
65  // get name of root and tip from the parameter server
66  std::string tip_name;
67  if (!node_.getParam("root_name", root_name_)){
68  ROS_ERROR("CartesianPoseController: No root name found on parameter server (namespace: %s)",
69  node_.getNamespace().c_str());
70  return false;
71  }
72  if (!node_.getParam("tip_name", tip_name)){
73  ROS_ERROR("CartesianPoseController: No tip name found on parameter server (namespace: %s)",
74  node_.getNamespace().c_str());
75  return false;
76  }
77 
78  // test if we got robot pointer
79  assert(robot_state);
80  robot_state_ = robot_state;
81 
82  // create robot chain from root to tip
83  if (!chain_.init(robot_state_, root_name_, tip_name))
84  return false;
85  if (!chain_.allCalibrated())
86  {
87  ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
88  return false;
89  }
91 
92  // create solver
98 
99  // create pid controller for the translation and for the rotation
100  control_toolbox::Pid pid_controller;
101  if (!pid_controller.init(ros::NodeHandle(node_,"fb_trans"))) return false;
102  for (unsigned int i = 0; i < 3; i++)
103  pid_controller_.push_back(pid_controller);
104  if (!pid_controller.init(ros::NodeHandle(node_,"fb_rot"))) return false;
105  for (unsigned int i = 0; i < 3; i++)
106  pid_controller_.push_back(pid_controller);
107 
108  // subscribe to pose commands
109  sub_command_.subscribe(node_, "command", 10);
111  sub_command_, tf_, root_name_, 10, node_));
112  command_filter_->registerCallback(boost::bind(&CartesianPoseController::command, this, _1));
113 
114  // realtime publisher for control state
117 
118  return true;
119 }
120 
122 {
123  // reset pid controllers
124  for (unsigned int i=0; i<6; i++)
125  pid_controller_[i].reset();
126 
127  // initialize desired pose/twist
128  twist_ff_ = Twist::Zero();
129  pose_desi_ = getPose();
131 
132  loop_count_ = 0;
133 }
134 
135 
136 
138 {
139  // get time
140  ros::Time time = robot_state_->getTime();
141  ros::Duration dt = time - last_time_;
142  last_time_ = time;
143 
144  // get current pose
145  pose_meas_ = getPose();
146 
147  // pose feedback into twist
149  KDL::Wrench wrench_desi;
150  for (unsigned int i=0; i<6; i++)
151  wrench_desi(i) = pid_controller_[i].computeCommand(twist_error_(i), dt);
152 
153  // get the chain jacobian
154  jac_solver_->JntToJac(jnt_pos_, jacobian_);
155 
156  // Converts the wrench into joint efforts with a jacbobian-transpose
157  for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++){
158  jnt_eff_(i) = 0;
159  for (unsigned int j=0; j<6; j++)
160  jnt_eff_(i) += (jacobian_(j,i) * wrench_desi(j));
161  }
162 
163  // set effort to joints
165 
166 
167  if (++loop_count_ % 100 == 0){
169  if (state_error_publisher_->trylock()){
170  state_error_publisher_->msg_.linear.x = twist_error_.vel(0);
171  state_error_publisher_->msg_.linear.y = twist_error_.vel(1);
172  state_error_publisher_->msg_.linear.z = twist_error_.vel(2);
173  state_error_publisher_->msg_.angular.x = twist_error_.rot(0);
174  state_error_publisher_->msg_.angular.y = twist_error_.rot(1);
175  state_error_publisher_->msg_.angular.z = twist_error_.rot(2);
176  state_error_publisher_->unlockAndPublish();
177  }
178  }
180  if (state_pose_publisher_->trylock()){
181  Pose tmp;
183  poseStampedTFToMsg(Stamped<Pose>(tmp, ros::Time::now(), root_name_), state_pose_publisher_->msg_);
184  state_pose_publisher_->unlockAndPublish();
185  }
186  }
187  }
188 }
189 
190 
191 
193 {
194  // get the joint positions and velocities
196 
197  // get cartesian pose
198  Frame result;
199  jnt_to_pose_solver_->JntToCart(jnt_pos_, result);
200 
201  return result;
202 }
203 
204 void CartesianPoseController::command(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
205 {
206  // convert message to transform
207  Stamped<Pose> pose_stamped;
208  poseStampedMsgToTF(*pose_msg, pose_stamped);
209 
210  // convert to reference frame of root link of the controller chain
211  tf_.transformPose(root_name_, pose_stamped, pose_stamped);
212  tf::poseTFToKDL(pose_stamped, pose_desi_);
213 }
214 
215 } // namespace
216 
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
void resize(unsigned int newNrOfColumns)
Vector vel
std::vector< control_toolbox::Pid > pid_controller_
void command(const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
void getPositions(std::vector< double > &)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
boost::scoped_ptr< tf::MessageFilter< geometry_msgs::PoseStamped > > command_filter_
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_command_
boost::scoped_ptr< KDL::ChainJntToJacSolver > jac_solver_
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
void poseTFToKDL(const tf::Pose &t, KDL::Frame &k)
bool init(const ros::NodeHandle &n, const bool quiet=false)
void poseKDLToTF(const KDL::Frame &k, tf::Pose &t)
Vector rot
const std::string & getNamespace() const
unsigned int getNrOfJoints() const
void resize(unsigned int newSize)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
bool getParam(const std::string &key, std::string &s) const
boost::scoped_ptr< realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped > > state_pose_publisher_
static Time now()
pr2_mechanism_model::RobotState * robot_state_
bool init(RobotState *robot_state, const std::string &root, const std::string &tip)
#define ROS_ERROR(...)
void toKDL(KDL::Chain &chain)
boost::scoped_ptr< realtime_tools::RealtimePublisher< geometry_msgs::Twist > > state_error_publisher_
void addEfforts(KDL::JntArray &)
boost::scoped_ptr< KDL::ChainFkSolverPos > jnt_to_pose_solver_


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Mon Jun 10 2019 14:26:26