cartesian_wrench_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 
35 #include <algorithm>
37 
38 
39 using namespace KDL;
40 
42 
43 namespace controller {
44 
45 CartesianWrenchController::CartesianWrenchController()
46 : robot_state_(NULL),
47  jnt_to_jac_solver_(NULL)
48 {}
49 
50 
51 
53 {
55 }
56 
57 
59 {
60  // test if we got robot pointer
61  assert(robot);
62  robot_state_ = robot;
63 
64  node_ = n;
65 
66  // get name of root and tip from the parameter server
67  std::string root_name, tip_name;
68  if (!node_.getParam("root_name", root_name)){
69  ROS_ERROR("CartesianWrenchController: No root name found on parameter server (namespace: %s)",
70  node_.getNamespace().c_str());
71  return false;
72  }
73  if (!node_.getParam("tip_name", tip_name)){
74  ROS_ERROR("CartesianWrenchController: No tip name found on parameter server (namespace: %s)",
75  node_.getNamespace().c_str());
76  return false;
77  }
78 
79  // create robot chain from root to tip
80  if (!chain_.init(robot_state_, root_name, tip_name)){
81  ROS_ERROR("Initializing chain from %s to %s failed", root_name.c_str(), tip_name.c_str());
82  return false;
83  }
84  if (!chain_.allCalibrated())
85  {
86  ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
87  return false;
88  }
90 
91  // create solver
96 
97 
98  // subscribe to wrench commands
99  sub_command_ = node_.subscribe<geometry_msgs::Wrench>
100  ("command", 1, &CartesianWrenchController::command, this);
101 
102  return true;
103 }
104 
106 {
107  // set desired wrench to 0
108  wrench_desi_ = Wrench::Zero();
109 }
110 
111 
112 
114 {
115  // check if joints are calibrated
116  if (!chain_.allCalibrated()){
117  return;
118  }
119 
120  // get joint positions
122 
123  // get the chain jacobian
125 
126  // convert the wrench into joint efforts
127  for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++){
128  jnt_eff_(i) = 0;
129  for (unsigned int j=0; j<6; j++)
130  jnt_eff_(i) += (jacobian_(j,i) * wrench_desi_(j));
131  }
132 
133  // set effort to joints
135 }
136 
137 
138 
139 void CartesianWrenchController::command(const geometry_msgs::WrenchConstPtr& wrench_msg)
140 {
141  // convert to wrench command
142  wrench_desi_.force(0) = wrench_msg->force.x;
143  wrench_desi_.force(1) = wrench_msg->force.y;
144  wrench_desi_.force(2) = wrench_msg->force.z;
145  wrench_desi_.torque(0) = wrench_msg->torque.x;
146  wrench_desi_.torque(1) = wrench_msg->torque.y;
147  wrench_desi_.torque(2) = wrench_msg->torque.z;
148 }
149 
150 }; // namespace
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
boost::scoped_ptr< KDL::ChainJntToJacSolver > jnt_to_jac_solver_
void resize(unsigned int newNrOfColumns)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void getPositions(std::vector< double > &)
void setEfforts(KDL::JntArray &)
Vector torque
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
double z() const
void command(const geometry_msgs::WrenchConstPtr &wrench_msg)
double y() const
double x() const
const std::string & getNamespace() const
unsigned int getNrOfJoints() const
Vector force
void resize(unsigned int newSize)
bool getParam(const std::string &key, std::string &s) const
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)


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Wed Jun 5 2019 19:34:03