iCub-main
Loading...
Searching...
No Matches
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
24#include <iCub/iKin/iKinHlp.h>
25#include <iCub/iKin/iKinSlv.h>
26
27using namespace std;
28using namespace yarp::os;
29using namespace yarp::sig;
30using namespace yarp::math;
31using namespace iCub::iKin;
32
33
34int 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.addVocab32(IKINSLV_VOCAB_CMD_GET);
69 cmd.addVocab32(IKINSLV_VOCAB_OPT_DOF);
70 rpc.write(cmd,reply);
71 cout<<"got dof: "<<reply.toString()<<endl;
72
73 cmd.clear();
74 cmd.addVocab32(IKINSLV_VOCAB_CMD_GET);
75 cmd.addVocab32(IKINSLV_VOCAB_OPT_POSE);
76 rpc.write(cmd,reply);
77 cout<<"got pose: "<<reply.toString()<<endl;
78
79 cmd.clear();
80 cmd.addVocab32(IKINSLV_VOCAB_CMD_GET);
81 cmd.addVocab32(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.addVocab32(IKINSLV_VOCAB_CMD_SET);
90 cmd.addVocab32(IKINSLV_VOCAB_OPT_MODE);
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;
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);
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
static yarp::os::Bottle * getEndEffectorPoseOption(const yarp::os::Bottle &b)
Retrieves the end-effector pose data.
Definition iKinHlp.cpp:156
static yarp::os::Bottle * getJointsOption(const yarp::os::Bottle &b)
Retrieves the joints configuration data.
Definition iKinHlp.cpp:163
static yarp::os::Bottle * getTargetOption(const yarp::os::Bottle &b)
Retrieves commanded target data from a bottle.
Definition iKinHlp.cpp:149
static void addTargetOption(yarp::os::Bottle &b, const yarp::sig::Vector &xd)
Appends to a bottle all data needed to command a target.
Definition iKinHlp.cpp:86
static void addDOFOption(yarp::os::Bottle &b, const yarp::sig::Vector &dof)
Appends to a bottle all data needed to reconfigure chain's dof.
Definition iKinHlp.cpp:93
virtual void close()
Stop the solver and dispose it.
Definition iKinSlv.cpp:1532
Derived class which implements the on-line solver for the chain torso+arm.
Definition iKinSlv.h:584
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition iKinSlv.cpp:1813
cmd
Definition dataTypes.h:30
#define IKINSLV_VOCAB_OPT_DOF
Definition iKinVocabs.h:28
#define IKINSLV_VOCAB_VAL_MODE_TRACK
Definition iKinVocabs.h:44
#define IKINSLV_VOCAB_OPT_POSE
Definition iKinVocabs.h:26
#define IKINSLV_VOCAB_OPT_MODE
Definition iKinVocabs.h:25
#define IKINSLV_VOCAB_CMD_SET
Definition iKinVocabs.h:17
#define IKINSLV_VOCAB_CMD_GET
Definition iKinVocabs.h:16
int main()
Definition main.cpp:34
Copyright (C) 2008 RobotCub Consortium.
out
Definition sine.m:8