iCub-main
gravityThread.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Marco Randazzo, Matteo Fumagalli
4  * email: marco.randazzo@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #ifndef GRAVITY_THREAD
20 #define GRAVITY_THREAD
21 
22 #include <yarp/os/BufferedPort.h>
23 #include <yarp/os/RFModule.h>
24 #include <yarp/os/Time.h>
25 #include <yarp/os/Network.h>
26 #include <yarp/os/PeriodicThread.h>
27 #include <yarp/os/Stamp.h>
28 #include <yarp/sig/Vector.h>
29 #include <yarp/dev/PolyDriver.h>
30 #include <yarp/dev/ControlBoardInterfaces.h>
32 #include <iCub/iDyn/iDyn.h>
33 #include <iCub/iDyn/iDynBody.h>
34 
35 using namespace yarp::os;
36 using namespace yarp::sig;
37 using namespace yarp::dev;
38 using namespace iCub::ctrl;
39 using namespace iCub::iDyn;
40 
44 
45 class gravityCompensatorThread: public yarp::os::PeriodicThread
46 {
47 private:
48 
49  std::string wholeBodyName;
50  BufferedPort<Vector> *port_inertial;
51  BufferedPort<Vector> *la_additional_offset;
52  BufferedPort<Vector> *ra_additional_offset;
53  BufferedPort<Vector> *ll_additional_offset;
54  BufferedPort<Vector> *rl_additional_offset;
55  BufferedPort<Vector> *to_additional_offset;
56 
57  BufferedPort<Vector> *left_arm_exec_torques;
58  BufferedPort<Vector> *right_arm_exec_torques;
59  BufferedPort<Vector> *left_leg_exec_torques;
60  BufferedPort<Vector> *right_leg_exec_torques;
61  BufferedPort<Vector> *torso_exec_torques;
62 
63  BufferedPort<Vector> *left_arm_gravity_torques;
64  BufferedPort<Vector> *right_arm_gravity_torques;
65  BufferedPort<Vector> *left_leg_gravity_torques;
66  BufferedPort<Vector> *right_leg_gravity_torques;
67  BufferedPort<Vector> *torso_gravity_torques;
68 
69  PolyDriver *ddLA;
70  PolyDriver *ddRA;
71  PolyDriver *ddH;
72  IEncoders *iencs_arm_left;
73  IEncoders *iencs_arm_right;
74  IEncoders *iencs_head;
75  IControlMode *iCtrlMode_arm_left;
76  IControlMode *iCtrlMode_arm_right;
77  IControlMode *iCtrlMode_torso;
78  IInteractionMode *iIntMode_arm_left;
79  IInteractionMode *iIntMode_arm_right;
80  IInteractionMode *iIntMode_torso;
81  IImpedanceControl *iImp_arm_left;
82  ITorqueControl *iTqs_arm_left;
83  IImpedanceControl *iImp_arm_right;
84  ITorqueControl *iTqs_arm_right;
85  IImpedanceControl *iImp_torso;
86  ITorqueControl *iTqs_torso;
87  thread_status_enum thread_status;
88 
89  PolyDriver *ddLL;
90  PolyDriver *ddRL;
91  PolyDriver *ddT;
92  IEncoders *iencs_leg_left;
93  IEncoders *iencs_leg_right;
94  IEncoders *iencs_torso;
95  IControlMode *iCtrlMode_leg_left;
96  IControlMode *iCtrlMode_leg_right;
97  IInteractionMode *iIntMode_leg_left;
98  IInteractionMode *iIntMode_leg_right;
99  IImpedanceControl *iImp_leg_left;
100  ITorqueControl *iTqs_leg_left;
101  IImpedanceControl *iImp_leg_right;
102  ITorqueControl *iTqs_leg_right;
103 
104  Vector encoders_arm_left;
105  Vector encoders_arm_right;
106  Vector encoders_head;
107 
108  Vector encoders_leg_left;
109  Vector encoders_leg_right;
110  Vector encoders_torso;
111 
112  Vector *inertial{};
113 
114  AWLinEstimator *linEstUp{};
115  AWQuadEstimator *quadEstUp{};
116  AWLinEstimator *linEstLow{};
117  AWQuadEstimator *quadEstLow{};
118 
119  int left_arm_ctrlJnt{};
120  int right_arm_ctrlJnt{};
121  int left_leg_ctrlJnt{};
122  int right_leg_ctrlJnt{};
123  int torso_ctrlJnt{};
124  int allJnt{};
125 
126  iCubWholeBody* icub;
127 
128  Vector q_head, dq_head, d2q_head;
129  Vector q_larm, dq_larm, d2q_larm;
130  Vector q_rarm, dq_rarm, d2q_rarm;
131  Vector all_q_up, all_dq_up, all_d2q_up;
132 
133  Vector q_torso, dq_torso, d2q_torso;
134  Vector q_lleg, dq_lleg, d2q_lleg;
135  Vector q_rleg, dq_rleg, d2q_rleg;
136  Vector all_q_low, all_dq_low, all_d2q_low;
137 
138  Vector w0,dw0,d2p0,Fend,Muend;
139  Vector F_LArm, F_RArm, F_iDyn_LArm, F_iDyn_RArm, Offset_LArm, Offset_RArm;
140  Vector F_LLeg, F_RLeg, F_iDyn_LLeg, F_iDyn_RLeg, Offset_LLeg, Offset_RLeg;
141  Matrix F_ext_up, F_ext_low;
142  Vector inertial_measurements;
143 
144  std::string side;
145  std::string part;
146 
147  Vector exec_torques_LA,exec_torques_RA,exec_torques_LL,exec_torques_RL,exec_torques_TO;
148  Vector gravity_torques_LA,gravity_torques_RA,gravity_torques_LL,gravity_torques_RL,gravity_torques_TO;
149  Vector externalcmd_torques_LA,externalcmd_torques_RA,externalcmd_torques_LL,externalcmd_torques_RL, externalcmd_torques_TO;
150  Vector ampli_LA, ampli_RA, ampli_LL, ampli_RL, ampli_TO;
151  bool isCalibrated;
152  bool inertial_enabled;
153 
154  Vector evalVelUp(const Vector &x);
155  Vector evalVelLow(const Vector &x);
156  Vector evalAccUp(const Vector &x);
157  Vector evalAccLow(const Vector &x);
158 
159  void init_upper();
160  void init_lower();
161  void setLowerMeasure();
162  void setUpperMeasure();
163 
164 public:
165 
168 
169  gravityCompensatorThread(std::string _wholeBodyName, int _rate, PolyDriver *_ddLA, PolyDriver *_ddRA, PolyDriver *_ddH, PolyDriver *_ddLL, PolyDriver *_ddRL, PolyDriver *_ddT, version_tag icub_type, bool _inertial_enabled);
170 
171  void setZeroJntAngVelAcc();
172  bool readAndUpdate(bool waitMeasure=false);
173  bool getLowerEncodersSpeedAndAcceleration();
174  bool getUpperEncodersSpeedAndAcceleration();
175  bool threadInit() override;
176  void feedFwdGravityControl(int part_ctrlJnt, const std::string& s_part, IControlMode *iCtrlMode, ITorqueControl *iTqs, IImpedanceControl *iImp, IInteractionMode *iIntMode, const Vector &command, bool releasing=false);
177  void run() override;
178  void threadRelease() override;
179 
181  {
182  return thread_status;
183  }
184 
185 };
186 
187 #endif
iCub::ctrl::AWQuadEstimator
Definition: adaptWinPolyEstimator.h:207
STATUS_DISCONNECTED
@ STATUS_DISCONNECTED
Definition: gravityThread.h:41
GRAVITY_COMPENSATION_ON
@ GRAVITY_COMPENSATION_ON
Definition: gravityThread.h:42
EXTERNAL_TRQ_ON
@ EXTERNAL_TRQ_ON
Definition: gravityThread.h:43
iDyn.h
EXTERNAL_TRQ_OFF
@ EXTERNAL_TRQ_OFF
Definition: gravityThread.h:43
gravityCompensatorThread::external_mode
int external_mode
Definition: gravityThread.h:167
thread_status_enum
thread_status_enum
Definition: gravityThread.h:41
yarp::dev
Definition: DebugInterfaces.h:52
iCub::iDyn
Definition: iDyn.h:81
gravityCompensatorThread::getThreadStatus
thread_status_enum getThreadStatus()
Definition: gravityThread.h:180
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
x
x
Definition: compute_ekf_sym.m:21
STATUS_OK
@ STATUS_OK
Definition: gravityThread.h:41
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::iDyn::version_tag
Definition: iDynBody.h:108
gravityCompensatorThread::gravity_mode
int gravity_mode
Definition: gravityThread.h:166
iDynBody.h
adaptWinPolyEstimator.h
iCub::ctrl::AWLinEstimator
Definition: adaptWinPolyEstimator.h:184
iCub::iDyn::iCubWholeBody
Definition: iDynBody.h:1471
gravityCompensatorThread
Definition: gravityThread.h:45
GRAVITY_COMPENSATION_OFF
@ GRAVITY_COMPENSATION_OFF
Definition: gravityThread.h:42