25
25
* barrett::units as you see fit!
26
26
*/
27
27
28
-
28
+ # include < cstdlib > // For strtod()
29
29
#include < iostream>
30
30
#include < string>
31
- #include < cstdlib> // For strtod()
32
31
33
32
// The file below provides access to the barrett::units namespace.
34
- #include < barrett/units.h>
35
- #include < barrett/systems.h>
36
33
#include < barrett/products/product_manager.h>
34
+ #include < barrett/systems.h>
35
+ #include < barrett/units.h>
37
36
38
37
#include < barrett/standard_main_function.h>
39
38
40
-
41
39
using namespace barrett ;
42
40
43
-
44
41
// This function template will accept a math::Matrix with any number of rows,
45
42
// any number of columns, and any units. In other words: it will accept any
46
43
// barrett::units type.
47
- template <int R, int C, typename Units>
48
- bool parseDoubles (math::Matrix<R,C, Units>* dest, const std::string& str) {
49
- const char * cur = str.c_str ();
50
- const char * next = cur;
44
+ template <int R, int C, typename Units>
45
+ bool parseDoubles (math::Matrix<R, C, Units> * dest, const std::string & str) {
46
+ const char * cur = str.c_str ();
47
+ const char * next = cur;
51
48
52
49
for (int i = 0 ; i < dest->size (); ++i) {
53
- (*dest)[i] = strtod (cur, (char **) &next);
50
+ (*dest)[i] = strtod (cur, (char **)&next);
54
51
if (cur == next) {
55
52
return false ;
56
53
} else {
@@ -59,8 +56,8 @@ bool parseDoubles(math::Matrix<R,C, Units>* dest, const std::string& str) {
59
56
}
60
57
61
58
// Make sure there are no extra numbers in the string.
62
- double ignore = strtod (cur, (char **) &next);
63
- (void )ignore; // Prevent unused variable warnings
59
+ double ignore = strtod (cur, (char **)&next);
60
+ (void )ignore; // Prevent unused variable warnings
64
61
65
62
if (cur != next) {
66
63
return false ;
@@ -69,16 +66,16 @@ bool parseDoubles(math::Matrix<R,C, Units>* dest, const std::string& str) {
69
66
return true ;
70
67
}
71
68
72
- template <size_t DOF, int R, int C, typename Units>
73
- void moveToStr (systems::Wam<DOF>& wam, math::Matrix<R,C, Units>* dest,
74
- const std::string& description, const std::string& str)
75
- {
69
+ template <size_t DOF, int R, int C, typename Units>
70
+ void moveToStr (systems::Wam<DOF> &wam, math::Matrix<R, C, Units> *dest,
71
+ const std::string &description, const std::string &str) {
76
72
if (parseDoubles (dest, str)) {
77
73
std::cout << " Moving to " << description << " : " << *dest << std::endl;
78
74
wam.moveTo (*dest);
79
75
} else {
80
76
printf (" ERROR: Please enter exactly %ld numbers separated by "
81
- " whitespace.\n " , dest->size ());
77
+ " whitespace.\n " ,
78
+ dest->size ());
82
79
}
83
80
}
84
81
@@ -91,18 +88,18 @@ void printMenu() {
91
88
printf (" q Quit\n " );
92
89
}
93
90
94
- template <size_t DOF>
95
- int wam_main (int argc, char ** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
91
+ template <size_t DOF>
92
+ int wam_main (int argc, char **argv, ProductManager &pm,
93
+ systems::Wam<DOF> &wam) {
96
94
// The macro below makes a number of typedefs that allow convenient access
97
95
// to commonly used barrett::units. For example, the typedefs establish
98
96
// "jp_type" as a synonym for "units::JointPositions<DOF>::type". This macro
99
97
// (along with a few others) is defined in barrett/units.h.
100
98
BARRETT_UNITS_TEMPLATE_TYPEDEFS (DOF);
101
99
102
100
// These vectors are fixed sized, stack allocated, and zero-initialized.
103
- jp_type jp; // jp is a DOFx1 column vector of joint positions
104
- cp_type cp; // cp is a 3x1 vector representing a Cartesian position
105
-
101
+ jp_type jp; // jp is a DOFx1 column vector of joint positions
102
+ cp_type cp; // cp is a 3x1 vector representing a Cartesian position
106
103
107
104
wam.gravityCompensate ();
108
105
printMenu ();
@@ -123,8 +120,8 @@ int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
123
120
break ;
124
121
125
122
case ' h' :
126
- std::cout << " Moving to home position: "
127
- << wam. getHomePosition () << std::endl;
123
+ std::cout << " Moving to home position: " << wam. getHomePosition ()
124
+ << std::endl;
128
125
wam.moveHome ();
129
126
break ;
130
127
@@ -148,7 +145,6 @@ int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
148
145
}
149
146
}
150
147
151
-
152
148
wam.idle ();
153
149
pm.getSafetyModule ()->waitForMode (SafetyModule::IDLE);
154
150
return 0 ;
0 commit comments