jog_api_example.cpp
Go to the documentation of this file.
1 // Title : jog_api_example.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 // Perform a motion with the jog_arm API
32 
34 
35 int main(int argc, char** argv)
36 {
37  ros::init(argc, argv, "jog_api_example");
38 
40  spinner.start();
41 
43  // Send motion commands with this JogAPI object.
44  // Put your robot name here -- often "manipulator"
46  std::string move_group_name = "manipulator";
47  JogAPI jogger(move_group_name, "/jog_arm_server/delta_jog_cmds");
48 
50  // Move to a good start pose
52  geometry_msgs::PoseStamped new_pose;
53  new_pose.header.frame_id = "world";
54  new_pose.header.stamp = ros::Time::now();
55  new_pose.pose.position.x = 0.4;
56  new_pose.pose.position.y = 0.23;
57  new_pose.pose.position.z = 0.4;
58  new_pose.pose.orientation.x = 0.025;
59  new_pose.pose.orientation.y = 0.247;
60  new_pose.pose.orientation.z = 0.283;
61  new_pose.pose.orientation.w = 0.926;
62 
63  // Speed scales for each dimension (x,y,z, Rx, Ry, Rz)
64  // Scale linear velocity commands between -0.5:0.5
65  // Scale angular velocity commands between -1.0 : 1.0
66  std::vector<double> speed_scale{ 0.5, 0.5, 0.5, 1.0, 1.0, 1.0 };
67 
68  // 1cm tolerance on the linear motion.
69  // 0.01rad tolerance on the angular
70  // Timeout, i.e. stop sending commands, after 10s
71  if (!jogger.jacobianMove(new_pose, 0.01, 0.01, speed_scale, ros::Duration(10)))
72  {
73  ROS_ERROR_STREAM("Jacobian move failed");
74  return 1;
75  }
76 
78  // Get robot's current pose via MoveIt's api
81  geometry_msgs::PoseStamped current_pose = mgi.getCurrentPose();
82 
83  ROS_INFO_STREAM("Current pose: " << current_pose);
84 
85  return 0;
86 }
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
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void spinner()
Definition: jog_api.h:43
geometry_msgs::PoseStamped getCurrentPose(const std::string &end_effector_link="")
#define ROS_INFO_STREAM(args)
static Time now()
#define ROS_ERROR_STREAM(args)
int main(int argc, char **argv)


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