19 #ifndef OBSERVER_THREAD
20 #define OBSERVER_THREAD
22 #include <yarp/os/all.h>
23 #include <yarp/sig/all.h>
24 #include <yarp/dev/all.h>
36 using namespace yarp::os;
37 using namespace yarp::sig;
38 using namespace yarp::math;
76 bool iCub_not_moving{};
87 inertial_w0.resize(3); inertial_w0.zero();
88 inertial_dw0.resize(3); inertial_dw0.zero();
89 inertial_d2p0.resize(3); inertial_d2p0.zero();
90 all_q_up.resize(3+7+7); all_q_up.zero();
91 all_dq_up.resize(3+7+7); all_dq_up.zero();
92 all_d2q_up.resize(3+7+7); all_d2q_up.zero();
93 all_q_low.resize(3+6+6); all_q_low.zero();
94 all_dq_low.resize(3+6+6); all_dq_low.zero();
95 all_d2q_low.resize(3+6+6); all_d2q_low.zero();
96 ft_arm_left.resize(6); ft_arm_left.zero();
97 ft_arm_right.resize(6); ft_arm_right.zero();
98 ft_leg_left.resize(6); ft_leg_left.zero();
99 ft_leg_right.resize(6); ft_leg_right.zero();
100 ft_foot_left.resize(6); ft_foot_left.zero();
101 ft_foot_right.resize(6); ft_foot_right.zero();
123 bool checkIcubNotMoving();
150 IEncoders *iencs_arm_left;
151 IEncoders *iencs_arm_right;
152 IEncoders *iencs_head;
153 IInteractionMode *iint_arm_left;
154 IInteractionMode *iint_arm_right;
155 IInteractionMode *iint_head;
156 IControlMode *icmd_arm_left;
157 IControlMode *icmd_arm_right;
158 IControlMode *icmd_head;
163 IEncoders *iencs_leg_left;
164 IEncoders *iencs_leg_right;
165 IEncoders *iencs_torso;
166 IInteractionMode *iint_leg_left;
167 IInteractionMode *iint_leg_right;
168 IInteractionMode *iint_torso;
169 IControlMode *icmd_leg_left;
170 IControlMode *icmd_leg_right;
171 IControlMode *icmd_torso;
175 const long double zero_sens_tolerance;
176 double skinContactsTimestamp;
179 BufferedPort<Vector> *port_ft_arm_left;
180 BufferedPort<Vector> *port_ft_arm_right;
181 BufferedPort<Vector> *port_ft_leg_left;
182 BufferedPort<Vector> *port_ft_leg_right;
183 BufferedPort<Vector> *port_ft_foot_left;
184 BufferedPort<Vector> *port_ft_foot_right;
185 BufferedPort<Vector> *port_inertial_thread;
186 BufferedPort<iCub::skinDynLib::skinContactList> *port_skin_contacts;
189 BufferedPort<Bottle> *port_RATorques;
190 BufferedPort<Bottle> *port_RLTorques;
191 BufferedPort<Bottle> *port_RWTorques;
192 BufferedPort<Bottle> *port_LATorques;
193 BufferedPort<Bottle> *port_LLTorques;
194 BufferedPort<Bottle> *port_LWTorques;
195 BufferedPort<Bottle> *port_TOTorques;
196 BufferedPort<Bottle> *port_HDTorques;
197 BufferedPort<Vector> *port_external_wrench_RA;
198 BufferedPort<Vector> *port_external_wrench_LA;
199 BufferedPort<Vector> *port_external_wrench_RL;
200 BufferedPort<Vector> *port_external_wrench_LL;
201 BufferedPort<Vector> *port_external_wrench_RF;
202 BufferedPort<Vector> *port_external_wrench_LF;
203 BufferedPort<Vector> *port_external_cartesian_wrench_RA;
204 BufferedPort<Vector> *port_external_cartesian_wrench_LA;
205 BufferedPort<Vector> *port_external_cartesian_wrench_RL;
206 BufferedPort<Vector> *port_external_cartesian_wrench_LL;
207 BufferedPort<Vector> *port_external_cartesian_wrench_RF;
208 BufferedPort<Vector> *port_external_cartesian_wrench_LF;
209 BufferedPort<Vector> *port_sensor_wrench_RL;
210 BufferedPort<Vector> *port_sensor_wrench_LL;
211 BufferedPort<Vector> *port_model_wrench_RL;
212 BufferedPort<Vector> *port_model_wrench_LL;
213 BufferedPort<Vector> *port_external_wrench_TO;
214 BufferedPort<Vector> *port_com_all;
215 BufferedPort<Vector> *port_com_all_foot;
216 BufferedPort<Vector> *port_com_lb;
217 BufferedPort<Vector> *port_com_ub;
218 BufferedPort<Vector> *port_com_la;
219 BufferedPort<Vector> *port_com_ra;
220 BufferedPort<Vector> *port_com_ll;
221 BufferedPort<Vector> *port_com_rl;
222 BufferedPort<Vector> *port_com_hd;
223 BufferedPort<Vector> *port_com_to;
224 BufferedPort<Vector> *port_monitor;
225 BufferedPort<iCub::skinDynLib::skinContactList> *port_contacts;
226 BufferedPort<Vector> *port_dumpvel;
227 BufferedPort<Vector> *port_COM_vel;
228 BufferedPort<Matrix> *port_COM_Jacobian;
229 BufferedPort<Vector> *port_all_velocities;
230 BufferedPort<Vector> *port_all_positions;
231 BufferedPort<Matrix> *port_root_position_mat;
232 BufferedPort<Vector> *port_root_position_vec;
235 BufferedPort<Vector> *port_external_ft_arm_left;
236 BufferedPort<Vector> *port_external_ft_arm_right;
237 BufferedPort<Vector> *port_external_ft_leg_left;
238 BufferedPort<Vector> *port_external_ft_leg_right;
239 yarp::os::Stamp timestamp;
256 size_t status_queue_size;
257 list<iCubStatus> previous_status;
258 list<iCubStatus> not_moving_status;
260 Vector encoders_arm_left;
261 Vector encoders_arm_right;
262 Vector encoders_head;
264 Vector encoders_leg_left;
265 Vector encoders_leg_right;
266 Vector encoders_torso;
269 Vector F_LArm, F_RArm;
270 Vector F_iDyn_LArm, F_iDyn_RArm, Offset_LArm, Offset_RArm;
271 Vector F_ext_left_arm, F_ext_right_arm, F_ext_torso;
272 Vector F_ext_cartesian_left_arm, F_ext_cartesian_right_arm;
273 Vector F_ext_left_leg, F_ext_right_leg;
274 Vector F_ext_left_foot, F_ext_right_foot;
275 Vector F_sns_left_leg, F_sns_right_leg;
276 Vector F_mdl_left_leg, F_mdl_right_leg;
277 Vector F_ext_cartesian_left_leg, F_ext_cartesian_right_leg;
278 Vector F_ext_cartesian_left_foot, F_ext_cartesian_right_foot;
279 Vector F_LLeg, F_RLeg;
280 Vector F_LFoot, F_RFoot;
281 Vector F_iDyn_LLeg, F_iDyn_RLeg, Offset_LLeg, Offset_RLeg;
282 Vector F_iDyn_LFoot, F_iDyn_RFoot, Offset_LFoot, Offset_RFoot;
283 Matrix F_sens_up, F_sens_low, F_ext_up, F_ext_low;
284 Vector F_ext_sens_right_arm, F_ext_sens_left_arm;
285 Vector F_ext_sens_right_leg, F_ext_sens_left_leg;
293 Matrix FM_sens_up,FM_sens_low;
298 Vector evalVelUp(
const Vector &
x);
299 Vector evalVelLow(
const Vector &
x);
300 Vector eval_domega(
const Vector &
x);
301 Vector evalAccUp(
const Vector &
x);
302 Vector evalAccLow(
const Vector &
x);
306 void setUpperMeasure(
bool _init=
false);
307 void setLowerMeasure(
bool _init=
false);
309 void addSkinContacts();
312 inverseDynamics(
int _rate, PolyDriver *_ddAL, PolyDriver *_ddAR, PolyDriver *_ddH, PolyDriver *_ddLL, PolyDriver *_ddLR, PolyDriver *_ddT,
string _robot_name,
string _local_name,
version_tag icub_type,
bool _autoconnect=
false );
313 bool threadInit()
override;
317 return thread_status;
321 void threadRelease()
override;
322 void closePort(Contactable *_port);
323 void writeTorque(Vector _values,
int _address, BufferedPort<Bottle> *_port);
324 template <
class T>
void broadcastData(
T& _values, BufferedPort<T> *_port);
326 bool readAndUpdate(
bool waitMeasure=
false,
bool _init=
false);
327 bool getLowerEncodersSpeedAndAcceleration();
328 bool getUpperEncodersSpeedAndAcceleration();
329 void setZeroJntAngVelAcc();
330 void sendMonitorData();
331 void sendVelAccData();
Adaptive window linear fitting to estimate the first derivative.
Adaptive window quadratic fitting to estimate the second derivative.
A class for connecting UpperTorso and LowerTorso of the iCub, then getting the WholeBody in the dynam...
thread_status_enum getThreadStatus()
double lpf_ord1_3hz(double input, int j)
constexpr int8_t MAX_FILTER_ORDER
constexpr float_t SKIN_EVENTS_TIMEOUT