iCub-main
tutorial_arm.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 #include <string>
4 #include <cstdio>
5 
6 #include <yarp/os/Network.h>
7 #include <yarp/dev/ControlBoardInterfaces.h>
8 #include <yarp/dev/PolyDriver.h>
9 #include <yarp/os/Time.h>
10 #include <yarp/sig/Vector.h>
11 
12 using namespace std;
13 using namespace yarp::dev;
14 using namespace yarp::sig;
15 using namespace yarp::os;
16 
17 int main(int argc, char *argv[])
18 {
19  Network yarp;
20 
21  Property params;
22  params.fromCommand(argc, argv);
23 
24  if (!params.check("robot"))
25  {
26  fprintf(stderr, "Please specify the name of the robot\n");
27  fprintf(stderr, "--robot name (e.g. icub)\n");
28  return 1;
29  }
30  std::string robotName=params.find("robot").asString();
31  std::string remotePorts="/";
32  remotePorts+=robotName;
33  remotePorts+="/right_arm";
34 
35  std::string localPorts="/test/client";
36 
37  Property options;
38  options.put("device", "remote_controlboard");
39  options.put("local", localPorts); //local port names
40  options.put("remote", remotePorts); //where we connect to
41 
42  // create a device
43  PolyDriver robotDevice(options);
44  if (!robotDevice.isValid()) {
45  printf("Device not available. Here are the known devices:\n");
46  printf("%s", Drivers::factory().toString().c_str());
47  return 0;
48  }
49 
50  IPositionControl *pos;
51  IEncoders *encs;
52 
53  bool ok;
54  ok = robotDevice.view(pos);
55  ok = ok && robotDevice.view(encs);
56 
57  if (!ok) {
58  printf("Problems acquiring interfaces\n");
59  return 0;
60  }
61 
62  int nj=0;
63  pos->getAxes(&nj);
64  Vector encoders;
65  Vector command;
66  Vector tmp;
67  encoders.resize(nj);
68  tmp.resize(nj);
69  command.resize(nj);
70 
71  int i;
72  for (i = 0; i < nj; i++) {
73  tmp[i] = 50.0;
74  }
75  pos->setRefAccelerations(tmp.data());
76 
77  for (i = 0; i < nj; i++) {
78  tmp[i] = 10.0;
79  pos->setRefSpeed(i, tmp[i]);
80  }
81 
82  //pos->setRefSpeeds(tmp.data()))
83 
84  //fisrst read all encoders
85  //
86  printf("waiting for encoders");
87  while(!encs->getEncoders(encoders.data()))
88  {
89  Time::delay(0.1);
90  printf(".");
91  }
92  printf("\n;");
93 
94  command=encoders;
95  //now set the shoulder to some value
96  command[0]=-50;
97  command[1]=20;
98  command[2]=-10;
99  command[3]=50;
100  pos->positionMove(command.data());
101 
102  bool done=false;
103 
104  while(!done)
105  {
106  pos->checkMotionDone(&done);
107  Time::delay(0.1);
108  }
109 
110  int times=0;
111  while(true)
112  {
113  times++;
114  if (times%2)
115  {
116  command[0]=-50;
117  command[1]=20;
118  command[2]=-10;
119  command[3]=50;
120  }
121  else
122  {
123  command[0]=-20;
124  command[1]=40;
125  command[2]=-10;
126  command[3]=30;
127  }
128 
129  pos->positionMove(command.data());
130 
131  int count=50;
132  while(count--)
133  {
134  Time::delay(0.1);
135  bool ret=encs->getEncoders(encoders.data());
136 
137  if (!ret)
138  {
139  fprintf(stderr, "Error receiving encoders, check connectivity with the robot\n");
140  }
141  else
142  {
143  printf("%.1lf %.1lf %.1lf %.1lf\n", encoders[0], encoders[1], encoders[2], encoders[3]);
144  }
145  }
146  }
147 
148  robotDevice.close();
149 
150  return 0;
151 }
const dReal * pos
Definition: iCub_Sim.cpp:67
STL namespace.
Copyright (C) 2008 RobotCub Consortium.
std::string toString(const T &t)
Definition: compensator.h:199
int main(int argc, char *argv[])
fprintf(fid,'\n')
bool done
Definition: main.cpp:42