jog_api.cpp
Go to the documentation of this file.
1 // Title : jog_api.cpp
3 // Project : jog_arm
4 // Created : 3/27/2018
5 // Author : Andy Zelenak
6 // Platforms : Ubuntu 64-bit
7 // Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
8 //
9 // All files within this directory are subject to the following, unless an alternative
10 // license is explicitly included within the text of each file.
11 //
12 // This software and documentation constitute an unpublished work
13 // and contain valuable trade secrets and proprietary information
14 // belonging to the University. None of the foregoing material may be
15 // copied or duplicated or disclosed without the express, written
16 // permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
17 // AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
18 // INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
19 // PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
20 // THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
21 // NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
22 // THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
23 // University be liable for incidental, special, indirect, direct or
24 // consequential damages or loss of profits, interruption of business,
25 // or related expenses which may arise from use of software or documentation,
26 // including but not limited to those resulting from defects in software
27 // and/or documentation, or loss or inaccuracy of data of any kind.
28 //
30 
31 // Provide a C++ interface for sending motion commands to the jog_arm server.
32 
33 #include "jog_api/jog_api.h"
34 
36 // Publish cmds for a Cartesian motion to bring the robot to the target pose.
37 // Param linear_vel_scale: scales the velocity components of outgoing Twist msgs. Should be 0<linear_vel_scale<1
38 // Return true if successful.
40 bool JogAPI::jacobianMove(geometry_msgs::PoseStamped& target_pose, const double trans_tolerance,
41  const double rot_tolerance, const std::vector<double>& speed_scale,
42  const ros::Duration& timeout)
43 {
44  // Velocity scaling should be between 0 and 1
45  for (int i = 0; i < speed_scale.size(); i++)
46  {
47  if (0. > speed_scale[i] || 1. < speed_scale[i])
48  {
49  ROS_ERROR_STREAM("[jog_api::jacobianMove] Velocity scaling parameter should be between 0 and 1.");
50  return false;
51  }
52  }
53 
54  geometry_msgs::PoseStamped current_pose;
55  current_pose = move_group_.getCurrentPose();
56  // Transform to target frame
57  transformPose(current_pose, target_pose.header.frame_id);
58 
59  // A structure to hold the result
61  distanceAndTwist = calculateDistanceAndTwist(current_pose, target_pose, speed_scale);
62 
63  ros::Time begin = ros::Time::now();
64 
65  // Is the current pose close enough?
66  while ((distanceAndTwist.translational_distance > trans_tolerance ||
67  distanceAndTwist.rotational_distance > rot_tolerance) &&
68  ros::ok())
69  {
70  // Have we timed out?
71  if (ros::Time::now() - begin > timeout)
72  return false;
73 
74  // Get current robot pose
75  current_pose = move_group_.getCurrentPose();
76 
77  transformPose(current_pose, target_pose.header.frame_id);
78 
79  // Update distance and twist to target
80  distanceAndTwist = calculateDistanceAndTwist(current_pose, target_pose, speed_scale);
81 
82  // Publish the twist commands to move the robot
83  jog_vel_pub_.publish(distanceAndTwist.twist);
84  ros::Duration(0.01).sleep();
85  }
86 
87  return true;
88 }
89 
91 // Maintain the current pose in given frame for given duration
93 bool JogAPI::maintainPose(std::string frame, const ros::Duration duration, const std::vector<double>& speed_scale)
94 {
95  // Velocity scaling should be between 0 and 1
96  for (int i = 0; i < speed_scale.size(); i++)
97  {
98  if (0. > speed_scale[i] || 1. < speed_scale[i])
99  {
100  ROS_ERROR_STREAM("[jog_api::maintainPose] Velocity scaling parameter should be between 0 and 1.");
101  return false;
102  }
103  }
104 
105  // Get initial pose -- to be maintained
106  geometry_msgs::PoseStamped initial_pose;
107  // The first call to this function often returns garbage. Do it twice.
108  initial_pose = move_group_.getCurrentPose();
109  initial_pose = move_group_.getCurrentPose();
110  transformPose(initial_pose, frame);
111 
112  ros::Time begin = ros::Time::now();
113  distanceAndTwist currentDistanceAndTwist, previousGoodDistanceAndTwist;
114  geometry_msgs::PoseStamped current_pose;
115 
116  while (ros::ok() && (ros::Time::now() < (begin + duration)))
117  {
118  // Get current robot pose
119  current_pose = move_group_.getCurrentPose();
120  transformPose(current_pose, frame);
121 
122  // Update distance and twist to target
123  currentDistanceAndTwist = calculateDistanceAndTwist(current_pose, initial_pose, speed_scale);
124 
125  // Check for nan's. Sometimes caused by calculateDistanceAndTwist
126  if ( ! (std::isnan(currentDistanceAndTwist.twist.twist.linear.x) || std::isnan(currentDistanceAndTwist.twist.twist.linear.y) || std::isnan(currentDistanceAndTwist.twist.twist.linear.z) ||
127  std::isnan(currentDistanceAndTwist.twist.twist.angular.x) || std::isnan(currentDistanceAndTwist.twist.twist.angular.y) || std::isnan(currentDistanceAndTwist.twist.twist.angular.z)))
128  {
129  // Publish the twist commands to move the robot
130  jog_vel_pub_.publish(currentDistanceAndTwist.twist);
131 
132  previousGoodDistanceAndTwist = currentDistanceAndTwist;
133  }
134  else
135  {
136  ROS_WARN_STREAM("[jog_api] nan in incoming command. Skipping this datapoint.");
137  jog_vel_pub_.publish(previousGoodDistanceAndTwist.twist);
138  }
139 
140  ros::Duration(0.01).sleep();
141  }
142 
143  // Ensure the robot stops
144  currentDistanceAndTwist.twist.twist.linear.x = 0;
145  currentDistanceAndTwist.twist.twist.linear.y = 0;
146  currentDistanceAndTwist.twist.twist.linear.z = 0;
147  currentDistanceAndTwist.twist.twist.angular.x = 0;
148  currentDistanceAndTwist.twist.twist.angular.y = 0;
149  currentDistanceAndTwist.twist.twist.angular.z = 0;
150  jog_vel_pub_.publish(currentDistanceAndTwist.twist);
151 
152  return true;
153 }
154 
156 // Transform a pose into given frame
158 bool JogAPI::transformPose(geometry_msgs::PoseStamped& pose, std::string& desired_frame)
159 {
160  // Remove a leading slash, if any
161  if (pose.header.frame_id.at(0) == '/')
162  pose.header.frame_id.erase(0, 1);
163  if (desired_frame.at(0) == '/')
164  desired_frame.erase(0, 1);
165 
166  try
167  {
168  geometry_msgs::TransformStamped current_frame_to_target =
169  tf_buffer_.lookupTransform(desired_frame, pose.header.frame_id, ros::Time(0), ros::Duration(1.0));
170  tf2::doTransform(pose, pose, current_frame_to_target);
171  pose.header.frame_id = desired_frame;
172  }
173  catch (tf2::TransformException& ex)
174  {
175  ROS_WARN("%s", ex.what());
176  ros::Duration(1.0).sleep();
177  }
178 
179  return true;
180 }
181 
183 // Calculate Euclidean distance between 2 poses
184 // Returns a distanceAndTwist structure.
185 // distanceAndTwist.translational_distance holds the linear distance to target pose
186 // distanceAndTwist.rotational_distance holds the rotational distance to target pose
187 // distanceAndTwist.twist: these components have been normalized between -1:1,
188 // they can serve as motion commands as if from a joystick.
190 JogAPI::distanceAndTwist JogAPI::calculateDistanceAndTwist(const geometry_msgs::PoseStamped& current_pose,
191  const geometry_msgs::PoseStamped& target_pose,
192  const std::vector<double>& speed_scale)
193 {
194  distanceAndTwist result;
195 
196  // Check frames on incoming PoseStampeds
197  if (current_pose.header.frame_id != target_pose.header.frame_id)
198  {
199  ROS_ERROR_STREAM("[JogAPI::distanceAndTwist] Incoming PoseStamped tf frames do not match.");
200  return result;
201  }
202  result.twist.header.frame_id = current_pose.header.frame_id;
203  result.twist.header.stamp = ros::Time::now();
204 
205  // Current pose: convert from quat to RPY
206  tf::Quaternion q(current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z,
207  current_pose.pose.orientation.w);
208  tf::Matrix3x3 m(q);
209  double curr_r, curr_p, curr_y;
210  m.getRPY(curr_r, curr_p, curr_y);
211 
212  // Target pose: convert from quat to RPY
213  q = tf::Quaternion(target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z,
214  target_pose.pose.orientation.w);
215  m = tf::Matrix3x3(q);
216  double targ_r, targ_p, targ_y;
217  m.getRPY(targ_r, targ_p, targ_y);
218 
220  // Calculate the twist to target_pose
222  // Linear
223  result.twist.twist.linear.x = target_pose.pose.position.x - current_pose.pose.position.x;
224  result.twist.twist.linear.y = target_pose.pose.position.y - current_pose.pose.position.y;
225  result.twist.twist.linear.z = target_pose.pose.position.z - current_pose.pose.position.z;
226 
227  // Angular
228  result.twist.twist.angular.x = targ_r - curr_r;
229  result.twist.twist.angular.y = targ_p - curr_p;
230  result.twist.twist.angular.z = targ_y - curr_y;
231 
233  // Normalize the twist to the target pose. Calculate distance while we're at it
235 
236  // Linear:
237  double sos = pow(result.twist.twist.linear.x, 2.); // Sum-of-squares
238  sos += pow(result.twist.twist.linear.y, 2.);
239  sos += pow(result.twist.twist.linear.z, 2.);
240  result.translational_distance = pow(sos, 0.5);
241 
242  result.twist.twist.linear.x = speed_scale[0] * result.twist.twist.linear.x / result.translational_distance;
243  result.twist.twist.linear.y = speed_scale[1] * result.twist.twist.linear.y / result.translational_distance;
244  result.twist.twist.linear.z = speed_scale[2] * result.twist.twist.linear.z / result.translational_distance;
245 
246  // Angular:
247  sos = pow(result.twist.twist.angular.x, 2.);
248  sos += pow(result.twist.twist.angular.y, 2.);
249  sos += pow(result.twist.twist.angular.z, 2.);
250  result.rotational_distance = pow(sos, 0.5);
251 
252  // Ignore angle for now
253  result.twist.twist.angular.x = speed_scale[3] * result.twist.twist.angular.x / result.rotational_distance;
254  result.twist.twist.angular.y = speed_scale[4] * result.twist.twist.angular.y / result.rotational_distance;
255  result.twist.twist.angular.z = speed_scale[5] * result.twist.twist.angular.z / result.rotational_distance;
256 
257  return result;
258 }
ros::Publisher jog_vel_pub_
Definition: jog_api.h:67
bool jacobianMove(geometry_msgs::PoseStamped &target_pose, const double trans_tolerance, const double rot_tolerance, const std::vector< double > &speed_scale, const ros::Duration &timeout)
Definition: jog_api.cpp:40
void publish(const boost::shared_ptr< M > &message) const
bool sleep() const
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
bool transformPose(geometry_msgs::PoseStamped &pose, std::string &desired_frame)
Definition: jog_api.cpp:158
distanceAndTwist calculateDistanceAndTwist(const geometry_msgs::PoseStamped &current_pose, const geometry_msgs::PoseStamped &target_pose, const std::vector< double > &speed_scale)
Definition: jog_api.cpp:190
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
#define ROS_WARN(...)
bool maintainPose(std::string frame, const ros::Duration duration, const std::vector< double > &speed_scale)
Definition: jog_api.cpp:93
moveit::planning_interface::MoveGroupInterface move_group_
Definition: jog_api.h:65
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
ROSCPP_DECL bool ok()
tf2_ros::Buffer tf_buffer_
Definition: jog_api.h:69
geometry_msgs::PoseStamped getCurrentPose(const std::string &end_effector_link="")
double rotational_distance
Definition: jog_api.h:77
#define ROS_WARN_STREAM(args)
static Time now()
geometry_msgs::TwistStamped twist
Definition: jog_api.h:78
#define ROS_ERROR_STREAM(args)
double translational_distance
Definition: jog_api.h:77


jog_api
Author(s): Andy Zelenak
autogenerated on Tue Nov 20 2018 03:39:03