34 const double PI = 3.14159265358979323846;
48 static std::vector<int> x,y,z,u,v,w;
49 static const int xArr[], yArr[], zArr[], uArr[], vArr[], wArr[];
56 int main(
int argc,
char *argv[]) {
59 std::cout <<
"---------------------------------\n";
60 std::cout <<
"usage for socket connection: kni_test CONFIGFILE IP_ADDR\n";
61 std::cout <<
"---------------------------------\n";
65 std::cout <<
"-----------------\n";
66 std::cout <<
"KNI_TEST DEMO STARTED\n";
67 std::cout <<
"-----------------\n";
74 std::unique_ptr<CCdlSocket>
device;
75 std::unique_ptr<CCplSerialCRC>
protocol;
82 std::cout <<
"-------------------------------------------\n";
83 std::cout <<
"success: port " << port <<
" open\n";
84 std::cout <<
"-------------------------------------------\n";
91 std::cout <<
"-------------------------------------------\n";
92 std::cout <<
"success: protocol class instantiated\n";
93 std::cout <<
"-------------------------------------------\n";
94 protocol->init(device.get());
95 std::cout <<
"-------------------------------------------\n";
96 std::cout <<
"success: communication with Katana initialized\n";
97 std::cout <<
"-------------------------------------------\n";
104 katana->create(argv[1], protocol.get());
108 std::cout <<
"ERROR: " << e.
message() << std::endl;
111 std::cout <<
"-------------------------------------------\n";
112 std::cout <<
"success: katana initialized\n";
113 std::cout <<
"-------------------------------------------\n";
116 katana->setMaximumLinearVelocity(60);
119 int nOfMot =
katana->getNumberOfMotors();
122 std::vector<TPoint>
points(0);
125 std::vector<int>
encoders(nOfMot, 0);
126 std::vector<double>
pose(6, 0.0);
128 for (
int i = 0; i< 6; i++){
136 std::cout <<
"- Calibrating Katana, please wait for termination..." << std::endl;
138 std::cout <<
" ...done." << std::endl;
141 std::cout <<
"- Check if Katana has position or current controllers..." << std::endl;
142 controller =
katana->getCurrentControllerType(1);
143 for(
short motorNumber = 1; motorNumber < nOfMot; ++motorNumber) {
144 tmp_int =
katana->getCurrentControllerType(motorNumber+1);
145 if (tmp_int != controller) {
146 std::cout <<
"*** ERROR: Katana has mixed controller types on its axes! ***" << std::endl;
150 std::cout <<
" Katana has all ";
151 controller == 0 ? std::cout <<
"position" : std::cout <<
"current";
152 std::cout <<
" controllers." << std::endl;
153 std::cout <<
" ...done." << std::endl;
156 if (controller == 1) {
157 std::cout <<
"- Read forces..." << std::endl;
158 for (
short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
159 std::cout <<
" Motor " << (motorNumber+1) <<
": " <<
katana->getForce(motorNumber+1) << std::endl;
161 std::cout <<
" ...done." << std::endl;
165 std::cout <<
"- Get Katana model name..." << std ::endl;
166 model =
katana->GetBase()->GetGNL()->modelName;
167 std::cout <<
" " << model << std::endl;
168 std::cout <<
" ...done." << std::endl;
171 std::cout <<
"- Switch robot off..." << std ::endl;
173 std::cout <<
" Robot off" << std::endl;
174 std::cout <<
" ...done." << std::endl;
177 std::cout <<
"- Read robot encoders..." << std ::endl;
178 std::cout <<
" Encoder values:";
179 katana->getRobotEncoders(encoders.begin(), encoders.end());
180 for (std::vector<int>::iterator i= encoders.begin(); i != encoders.end(); ++i) {
181 std::cout <<
" " << *i;
183 std::cout << std::endl;
184 std::cout <<
" ...done." << std::endl;
187 std::cout <<
"- Switch motors on..." << std ::endl;
188 for (
short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
189 katana->switchMotorOn((
short)motorNumber);
190 std::cout <<
" Motor " << (motorNumber+1) <<
" on" << std::endl;
192 std::cout <<
" ...done." << std::endl;
195 std::cout <<
"- Move single axes..." << std ::endl;
196 katana->dec(0, 10000,
true);
197 std::cout <<
" Motor 1 decreased by 10000 encoders" << std::endl;
198 katana->inc(1, 10000,
true);
199 std::cout <<
" Motor 2 increased by 10000 encoders" << std::endl;
200 katana->decDegrees(2, 70,
true);
201 std::cout <<
" Motor 3 decreased by 70 degrees" << std::endl;
202 katana->mov(3, 20000,
true);
203 std::cout <<
" " <<
"Motor 4 moved to encoder position 20000" << std::endl;
204 katana->movDegrees(4, 90,
true);
205 std::cout <<
" Motor 5 moved to position 90 degrees" << std::endl;
206 katana->incDegrees(5, -35,
true);
207 std::cout <<
" Motor 6 increased by -35 degrees" << std::endl;
208 katana->moveMotorBy(0, 0.2,
true);
209 std::cout <<
" Motor 1 moved by 0.2 rad" << std::endl;
210 katana->moveMotorByEnc(1, -5000,
true);
211 std::cout <<
" Motor 2 moved by -5000 encoders" << std::endl;
212 katana->moveMotorTo(2, 3.1,
true);
213 std::cout <<
" Motor 3 moved to 3.1 rad" << std::endl;
214 katana->moveMotorToEnc(3, 10000,
true);
215 std::cout <<
" Motor 4 moved to 10000 encoders" << std::endl;
216 std::cout <<
" ...done." << std::endl;
219 std::cout <<
"- Move all axes..." << std ::endl;
220 for (
short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
221 encoders[motorNumber] = 30500;
222 if (motorNumber == 1 || motorNumber == 2)
223 encoders[motorNumber] = -30500;
225 katana->moveRobotToEnc(encoders,
true);
226 std::cout <<
" Robot moved to encoder position 30500 -30500 -30500 30500 30500 30500" << std::endl;
227 std::cout <<
" ...done." << std::endl;
230 std::cout <<
"- Get coordinates..." << std ::endl;
233 katana->getCoordinates(x, y, z, phi, theta, psi);
234 std::cout <<
" Current coordinates: " << x <<
" " << y <<
" " << z <<
" " << phi <<
" " << theta <<
" " << psi << std::endl;
236 std::cout <<
" ...done." << std::endl;
239 std::cout <<
"- Get coordinates from given encoders..." << std ::endl;
241 encoders[0] = encoders[3] = encoders[4] = encoders[5] = 25000;
242 encoders[1] = encoders[2] = -25000;
243 katana->getCoordinatesFromEncoders(pose, encoders);
244 std::cout <<
" Encoders: 25000 -25000 -25000 25000 25000 25000" << std::endl;
245 std::cout <<
" Coordinates: " << pose[0] <<
" " << pose[1] <<
" " << pose[2] <<
" " << pose[3] <<
" " << pose[4] <<
" " << pose[5] << std::endl;
247 std::cout <<
" ...done." << std::endl;
250 std::cout <<
"- Calculate inverse kinematics..." << std ::endl;
252 katana->IKCalculate(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], encoders.begin());
253 std::cout <<
" encoders.size(): " << encoders.size() << std::endl;
254 std::cout <<
" Possible encoders: " << encoders[0] <<
" " << encoders[1] <<
" " << encoders[2] <<
" " << encoders[3] <<
" " << encoders[4] <<
" " << encoders[5] << std::endl;
256 std::cout <<
" ...done." << std::endl;
259 std::cout <<
"- Move robot to pose..." << std ::endl;
260 katana->moveRobotTo(pose,
true);
261 std::cout <<
" Coordinates: " << pose[0] <<
" " << pose[1] <<
" " << pose[2] <<
" " << pose[3] <<
" " << pose[4] <<
" " << pose[5] << std::endl;
262 std::cout <<
" ...done." << std::endl;
267 cout <<
"- Load points file..." << endl;
268 ifstream listfile(model);
270 cout <<
"*** ERROR: File '" << model <<
"' not found or access denied! ***" << endl;
274 vector<string> tokens;
275 const string delimiter =
",";
277 while(!listfile.eof()) {
279 string::size_type lastPos = line.find_first_not_of(delimiter, 0);
280 string::size_type pos = line.find_first_of(delimiter, lastPos);
281 while (string::npos != pos || string::npos != lastPos) {
283 tokens.push_back(line.substr(lastPos, pos - lastPos));
285 lastPos = line.find_first_not_of(delimiter, pos);
287 pos = line.find_first_of(delimiter, lastPos);
290 point.
X = atof((tokens.at(0)).data());
291 point.
Y = atof((tokens.at(1)).data());
292 point.
Z = atof((tokens.at(2)).data());
293 point.
phi = atof((tokens.at(3)).data());
294 point.
theta = atof((tokens.at(4)).data());
295 point.
psi = atof((tokens.at(5)).data());
296 points.push_back( point );
300 cout <<
" " << lines <<
" points loaded." << endl;
301 cout <<
" ...done." << endl;
305 std::cout <<
"- Move in point list..." << std ::endl;
309 std::cout <<
" Single p2p movements..." << std::flush;
310 for (i = 0; i < (int)points.size(); ++i) {
311 katana->moveRobotTo(points[i].
X, points[i].
Y, points[i].
Z, points[i].
phi, points[i].
theta, points[i].
psi);
313 std::cout <<
" ...done." << std::endl;
314 std::cout <<
" Single linear movements..." << std::flush;
315 for (i = 0; i < (int)points.size(); ++i) {
316 katana->moveRobotLinearTo(points[i].
X, points[i].
Y, points[i].
Z, points[i].
phi, points[i].
theta, points[i].
psi);
318 std::cout <<
" ...done." << std::endl;
319 std::cout <<
" Concatenated p2p movements..." << std::flush;
320 for (i = 0; i < (int)points.size(); ++i) {
321 j = (i + points.size() - 1) % points.size();
323 if (i == ((
int)points.size()-1)) wait =
true;
324 katana->movP2P(points[j].
X, points[j].
Y, points[j].
Z, points[j].
phi, points[j].
theta, points[j].
psi, points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi,
true, 70.0, wait);
326 std::cout <<
" ...done." << std::endl;
327 std::cout <<
" Concatenated linear movements..." << std::flush;
328 for (i = 0; i < (int)points.size(); ++i) {
329 j = (i + points.size() - 1) % points.size();
331 if (i == ((
int)points.size()-1)) wait =
true;
332 katana->movLM2P(points[j].
X, points[j].
Y, points[j].
Z, points[j].
phi, points[j].
theta, points[j].
psi, points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi,
true, 100.0, wait);
334 std::cout <<
" ...done." << std::endl;
336 std::cout <<
" ...done." << std::endl;
339 std::cout <<
"- Move all axes..." << std ::endl;
340 for (
short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
341 encoders[motorNumber] = 30500;
342 if (motorNumber == 1 || motorNumber == 2)
343 encoders[motorNumber] = -30500;
345 katana->moveRobotToEnc(encoders,
true);
346 std::cout <<
" Robot moved to encoder position 30500 -30500 -30500 30500 30500 30500" << std::endl;
347 std::cout <<
" ...done." << std::endl;
static std::unique_ptr< CCdlSocket > device
static std::unique_ptr< CCplSerialCRC > protocol
std::string message() const
Implement the Serial-Zero protocol.
int main(int argc, char *argv[])
std::unique_ptr< CLMBase > katana
Encapsulates the socket communication device.
structure for the currently active axis
std::vector< TPoint > points(0)
std::vector< int > encoders