iCub-main
main.cpp
Go to the documentation of this file.
1 
13 #include <string>
14 #include <iostream>
15 #include <iomanip>
16 
17 #include <yarp/os/Network.h>
18 #include <yarp/os/Port.h>
19 #include <yarp/os/Bottle.h>
20 #include <yarp/sig/Vector.h>
21 #include <yarp/math/Math.h>
22 
23 #include <iCub/iKin/iKinVocabs.h>
24 #include <iCub/iKin/iKinHlp.h>
25 #include <iCub/iKin/iKinSlv.h>
26 
27 using namespace std;
28 using namespace yarp::os;
29 using namespace yarp::sig;
30 using namespace yarp::math;
31 using namespace iCub::iKin;
32 
33 
34 int main()
35 {
36  Bottle cmd, reply;
37  Network yarp;
38 
39  if (!yarp.checkNetwork())
40  return 1;
41 
42  // declare the on-line arm solver called "solver"
43  iCubArmCartesianSolver onlineSolver("solver");
44 
45  Property options;
46  // it will operate on the simulator (which is supposed to be already running)
47  options.put("robot","icubSim");
48  // it will work with the right arm
49  options.put("type","right");
50  // it will achieve just the positional pose
51  options.put("pose","xyz");
52  // switch off verbosity
53  options.put("verbosity","off");
54 
55  // launch the solver and let it connect to the simulator
56  if (!onlineSolver.open(options))
57  return 1;
58 
59  // prepare ports
60  Port in, out, rpc;
61  in.open("/in"); out.open("/out"); rpc.open("/rpc");
62  Network::connect("/solver/out",in.getName());
63  Network::connect(out.getName(),"/solver/in");
64  Network::connect(rpc.getName(),"/solver/rpc");
65 
66  // print status
67  cmd.clear();
68  cmd.addVocab(IKINSLV_VOCAB_CMD_GET);
69  cmd.addVocab(IKINSLV_VOCAB_OPT_DOF);
70  rpc.write(cmd,reply);
71  cout<<"got dof: "<<reply.toString()<<endl;
72 
73  cmd.clear();
74  cmd.addVocab(IKINSLV_VOCAB_CMD_GET);
75  cmd.addVocab(IKINSLV_VOCAB_OPT_POSE);
76  rpc.write(cmd,reply);
77  cout<<"got pose: "<<reply.toString()<<endl;
78 
79  cmd.clear();
80  cmd.addVocab(IKINSLV_VOCAB_CMD_GET);
81  cmd.addVocab(IKINSLV_VOCAB_OPT_MODE);
82  rpc.write(cmd,reply);
83  cout<<"got mode: "<<reply.toString()<<endl;
84 
85  // change to tracking mode so that when
86  // any movement induced on unactuated joints
87  // is detected the solver is able to react
88  cmd.clear();
89  cmd.addVocab(IKINSLV_VOCAB_CMD_SET);
90  cmd.addVocab(IKINSLV_VOCAB_OPT_MODE);
91  cmd.addVocab(IKINSLV_VOCAB_VAL_MODE_TRACK);
92  cout<<"switching to track mode...";
93  rpc.write(cmd,reply);
94  cout<<reply.toString()<<endl;
95 
96  // ask to resolve for some xyz position
97  cmd.clear();
98  Vector xd(3);
99  xd[0]=-0.3;
100  xd[1]=0.0;
101  xd[2]=0.1;
102  CartesianHelper::addTargetOption(cmd,xd);
103  out.write(cmd);
104  in.read(reply);
105 
106  cout<<"xd ="<<CartesianHelper::getTargetOption(reply)->toString()<<endl;
107  cout<<"x ="<<CartesianHelper::getEndEffectorPoseOption(reply)->toString()<<endl;
108  cout<<"q [deg] ="<<CartesianHelper::getJointsOption(reply)->toString()<<endl;
109  cout<<endl;
110 
111  // ask the same but with torso enabled
112  Vector dof(3,1.0);
113  CartesianHelper::addDOFOption(cmd,dof);
114  out.write(cmd);
115  in.read(reply);
116 
117  cout<<"xd ="<<CartesianHelper::getTargetOption(reply)->toString()<<endl;
118  cout<<"x ="<<CartesianHelper::getEndEffectorPoseOption(reply)->toString()<<endl;
119  cout<<"q [deg] ="<<CartesianHelper::getJointsOption(reply)->toString()<<endl;
120  cout<<endl;
121 
122  // close up
123  onlineSolver.close();
124  in.close();
125  out.close();
126  rpc.close();
127 
128  return 0;
129 }
130 
131 
132 
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition: iKinSlv.cpp:1820
out
Definition: sine.m:8
STL namespace.
#define IKINSLV_VOCAB_CMD_SET
Definition: iKinVocabs.h:17
Copyright (C) 2008 RobotCub Consortium.
cmd
Definition: dataTypes.h:30
#define IKINSLV_VOCAB_VAL_MODE_TRACK
Definition: iKinVocabs.h:44
#define IKINSLV_VOCAB_CMD_GET
Definition: iKinVocabs.h:16
virtual void close()
Stop the solver and dispose it.
Definition: iKinSlv.cpp:1539
#define IKINSLV_VOCAB_OPT_DOF
Definition: iKinVocabs.h:28
Derived class which implements the on-line solver for the chain torso+arm.
Definition: iKinSlv.h:569
int main(int argc, char *argv[])
Definition: main.cpp:31
#define IKINSLV_VOCAB_OPT_MODE
Definition: iKinVocabs.h:25
#define IKINSLV_VOCAB_OPT_POSE
Definition: iKinVocabs.h:26