NimbRo ROS Soccer Package
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
RobotModelVis.h
1 // ROS visualisation marker helper for RobotModel class
2 // File: RobotModelVis.h
3 // Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>
4 
5 // Ensure header is only included once
6 #ifndef ROBOTMODELVIS_H
7 #define ROBOTMODELVIS_H
8 
9 // Includes
10 #include <cap_gait/contrib/RobotModel.h>
11 #include <vis_utils/marker_manager.h>
12 
13 // Defines
14 #define NS_BODY "body"
15 #define NS_FOOTSTEP "footstep"
16 #define NS_SUPP_ARR "support_arrows"
17 #define NS_FREE_ARR "free_arrows"
18 
19 // Namespace
20 namespace margait_contrib
21 {
29  class RobotModelVis : public vis_utils::MarkerManager
30  {
31  public:
32  // Enumerations
33  enum JointsEnum
34  {
35  JNT_LSHOULDER,
36  JNT_LELBOW,
37  JNT_RSHOULDER,
38  JNT_RELBOW,
39  JNT_LHIP,
40  JNT_LKNEE,
41  JNT_LANKLE,
42  JNT_RHIP,
43  JNT_RKNEE,
44  JNT_RANKLE,
45  JNT_COUNT
46  };
47  enum LinesEnum
48  {
49  LIN_TRUNK_A,
50  LIN_TRUNK_B,
51  LIN_NECK_A,
52  LIN_NECK_B,
53  LIN_SHOULDER_A,
54  LIN_SHOULDER_B,
55  LIN_LUARM_A,
56  LIN_LUARM_B,
57  LIN_LLARM_A,
58  LIN_LLARM_B,
59  LIN_RUARM_A,
60  LIN_RUARM_B,
61  LIN_RLARM_A,
62  LIN_RLARM_B,
63  LIN_HIP_A,
64  LIN_HIP_B,
65  LIN_LTHIGH_A,
66  LIN_LTHIGH_B,
67  LIN_RTHIGH_A,
68  LIN_RTHIGH_B,
69  LIN_LSHANK_A,
70  LIN_LSHANK_B,
71  LIN_RSHANK_A,
72  LIN_RSHANK_B,
73  LIN_LFOOTX_A,
74  LIN_LFOOTX_B,
75  LIN_LFOOTY_A,
76  LIN_LFOOTY_B,
77  LIN_LFOOTZ_A,
78  LIN_LFOOTZ_B,
79  LIN_RFOOTX_A,
80  LIN_RFOOTX_B,
81  LIN_RFOOTY_A,
82  LIN_RFOOTY_B,
83  LIN_RFOOTZ_A,
84  LIN_RFOOTZ_B,
85  LIN_SUPP_F_A,
86  LIN_SUPP_F_B,
87  LIN_SUPP_R_A,
88  LIN_SUPP_R_B,
89  LIN_SUPP_B_A,
90  LIN_SUPP_B_B,
91  LIN_SUPP_L_A,
92  LIN_SUPP_L_B,
93  LIN_SUPN_F_A,
94  LIN_SUPN_F_B,
95  LIN_SUPN_R_A,
96  LIN_SUPN_R_B,
97  LIN_SUPN_B_A,
98  LIN_SUPN_B_B,
99  LIN_SUPN_L_A,
100  LIN_SUPN_L_B,
101  LIN_COUNT
102  };
103  enum FSLinesEnum
104  {
105  FIN_LFOOTSTEP_F_A,
106  FIN_LFOOTSTEP_F_B,
107  FIN_LFOOTSTEP_R_A,
108  FIN_LFOOTSTEP_R_B,
109  FIN_LFOOTSTEP_B_A,
110  FIN_LFOOTSTEP_B_B,
111  FIN_LFOOTSTEP_L_A,
112  FIN_LFOOTSTEP_L_B,
113  FIN_LFOOTSTEP_X_A,
114  FIN_LFOOTSTEP_X_B,
115  FIN_LFOOTSTEP_Y_A,
116  FIN_LFOOTSTEP_Y_B,
117  FIN_RFOOTSTEP_F_A,
118  FIN_RFOOTSTEP_F_B,
119  FIN_RFOOTSTEP_R_A,
120  FIN_RFOOTSTEP_R_B,
121  FIN_RFOOTSTEP_B_A,
122  FIN_RFOOTSTEP_B_B,
123  FIN_RFOOTSTEP_L_A,
124  FIN_RFOOTSTEP_L_B,
125  FIN_RFOOTSTEP_X_A,
126  FIN_RFOOTSTEP_X_B,
127  FIN_RFOOTSTEP_Y_A,
128  FIN_RFOOTSTEP_Y_B,
129  FIN_COUNT
130  };
131 
132  // Constructor
133  RobotModelVis(const RobotModel* model, const std::string& frame) : MarkerManager("~/cap_gait/robot_model", 1)
134  , model(model)
135  , frame(frame)
136  , Com(this, frame, 1, NS_BODY)
137  , Base(this, frame, 1, NS_BODY)
138  , Head(this, frame, 1, NS_BODY)
139  , Trunk(this, frame, 1, NS_BODY)
140  , Chest(this, frame, 1, NS_BODY)
141  , SuppCom(this, frame, 1, 1, 1, NS_SUPP_ARR)
142  , SuppStep(this, frame, 1, 1, 1, NS_SUPP_ARR)
143  , FreeCom(this, frame, 1, 1, 1, NS_FREE_ARR)
144  , FreeStep(this, frame, 1, 1, 1, NS_FREE_ARR)
145  , Joints(this, frame, NS_BODY)
146  , Lines(this, frame, NS_BODY)
147  , FSLines(this, frame, NS_FOOTSTEP)
148  , offsetX(0.0)
149  , offsetY(0.0)
150  , offsetZ(0.0)
151  {
152  // Create an empty geometry_msgs point
153  geometry_msgs::Point pt;
154  pt.x = pt.y = pt.z = 0.0;
155 
156  // Create a default colour type
157  visualization_msgs::Marker::_color_type col;
158  col.r = 0.0; col.g = 0.0; col.b = 0.0; col.a = 1.0;
159 
160  // CoM marker
161  Com.setColor(0.8, 0.2, 0.2);
162  Com.setScale(0.050);
163 
164  // Base marker
165  Base.setColor(0.5, 0.0, 0.5);
166  Base.setScale(0.050);
167 
168  // Head marker
169  Head.setColor(0.8, 0.8, 0.8);
170  Head.setScale(0.1);
171 
172  // Trunk marker
173  Trunk.setColor(0.3, 0.3, 0.3);
174  Trunk.setScale(0.050);
175 
176  // Chest marker
177  Chest.setColor(0.5, 0.0, 0.5);
178  Chest.setScale(0.050);
179 
180  // Arrow markers
181  SuppCom.setColor(0.8, 0.8, 0.0);
182  SuppCom.setScale(0.025, 0.050, 0.050);
183  SuppStep.setColor(0.8, 0.8, 0.0);
184  SuppStep.setScale(0.025, 0.050, 0.050);
185  FreeCom.setColor(1.0, 0.5, 0.0);
186  FreeCom.setScale(0.025, 0.050, 0.050);
187  FreeStep.setColor(1.0, 0.5, 0.0);
188  FreeStep.setScale(0.025, 0.050, 0.050);
189 
190  // Joint markers
191  Joints.setType(visualization_msgs::Marker::SPHERE_LIST);
192  Joints.marker.points.assign(JNT_COUNT, pt);
193  Joints.setColor(0.6, 0.3, 0.0);
194  Joints.setScale(0.050);
195 
196  // Line markers
197  Lines.setType(visualization_msgs::Marker::LINE_LIST);
198  Lines.marker.points.assign(LIN_COUNT, pt);
199  Lines.setScale(0.010);
200 
201  // Line markers - Colours
202  visualization_msgs::Marker::_colors_type& cols = Lines.marker.colors;
203  cols.assign(LIN_COUNT, col);
204  cols[LIN_TRUNK_A].b = 1.0;
205  cols[LIN_TRUNK_B].b = 1.0;
206  cols[LIN_NECK_A].r = 1.0;
207  cols[LIN_NECK_B].r = 1.0;
208  cols[LIN_SHOULDER_A].g = 1.0;
209  cols[LIN_SHOULDER_B].g = 1.0;
210  cols[LIN_LUARM_A].r = 0.5;
211  cols[LIN_LUARM_A].b = 0.5;
212  cols[LIN_LUARM_B].r = 0.5;
213  cols[LIN_LUARM_B].b = 0.5;
214  cols[LIN_LLARM_A].r = 0.5;
215  cols[LIN_LLARM_A].b = 0.5;
216  cols[LIN_LLARM_B].r = 0.5;
217  cols[LIN_LLARM_B].b = 0.5;
218  cols[LIN_RUARM_A].r = 0.5;
219  cols[LIN_RUARM_A].b = 0.5;
220  cols[LIN_RUARM_B].r = 0.5;
221  cols[LIN_RUARM_B].b = 0.5;
222  cols[LIN_RLARM_A].r = 0.5;
223  cols[LIN_RLARM_A].b = 0.5;
224  cols[LIN_RLARM_B].r = 0.5;
225  cols[LIN_RLARM_B].b = 0.5;
226  cols[LIN_HIP_A].g = 1.0;
227  cols[LIN_HIP_B].g = 1.0;
228  cols[LIN_LTHIGH_A].r = 0.5;
229  cols[LIN_LTHIGH_B].r = 0.5;
230  cols[LIN_RTHIGH_A].r = 0.5;
231  cols[LIN_RTHIGH_B].r = 0.5;
232  cols[LIN_LSHANK_A].r = 0.5;
233  cols[LIN_LSHANK_B].r = 0.5;
234  cols[LIN_RSHANK_A].r = 0.5;
235  cols[LIN_RSHANK_B].r = 0.5;
236  cols[LIN_LTHIGH_A].b = 0.5;
237  cols[LIN_LTHIGH_B].b = 0.5;
238  cols[LIN_RTHIGH_A].b = 0.5;
239  cols[LIN_RTHIGH_B].b = 0.5;
240  cols[LIN_LSHANK_A].b = 0.5;
241  cols[LIN_LSHANK_B].b = 0.5;
242  cols[LIN_RSHANK_A].b = 0.5;
243  cols[LIN_RSHANK_B].b = 0.5;
244  cols[LIN_LFOOTX_A].r = 1.0;
245  cols[LIN_LFOOTX_B].r = 1.0;
246  cols[LIN_LFOOTY_A].g = 1.0;
247  cols[LIN_LFOOTY_B].g = 1.0;
248  cols[LIN_LFOOTZ_A].b = 1.0;
249  cols[LIN_LFOOTZ_B].b = 1.0;
250  cols[LIN_RFOOTX_A].r = 1.0;
251  cols[LIN_RFOOTX_B].r = 1.0;
252  cols[LIN_RFOOTY_A].g = 1.0;
253  cols[LIN_RFOOTY_B].g = 1.0;
254  cols[LIN_RFOOTZ_A].b = 1.0;
255  cols[LIN_RFOOTZ_B].b = 1.0;
256  cols[LIN_SUPP_F_A].g = 0.8;
257  cols[LIN_SUPP_F_B].g = 0.8;
258  cols[LIN_SUPP_R_A].g = 0.8;
259  cols[LIN_SUPP_R_B].g = 0.8;
260  cols[LIN_SUPP_B_A].g = 0.8;
261  cols[LIN_SUPP_B_B].g = 0.8;
262  cols[LIN_SUPP_L_A].g = 0.8;
263  cols[LIN_SUPP_L_B].g = 0.8;
264  cols[LIN_SUPP_F_A].b = 0.8;
265  cols[LIN_SUPP_F_B].b = 0.8;
266  cols[LIN_SUPP_R_A].b = 0.8;
267  cols[LIN_SUPP_R_B].b = 0.8;
268  cols[LIN_SUPP_B_A].b = 0.8;
269  cols[LIN_SUPP_B_B].b = 0.8;
270  cols[LIN_SUPP_L_A].b = 0.8;
271  cols[LIN_SUPP_L_B].b = 0.8;
272  cols[LIN_SUPN_F_A].b = 1.0;
273  cols[LIN_SUPN_F_B].b = 1.0;
274  cols[LIN_SUPN_R_A].b = 1.0;
275  cols[LIN_SUPN_R_B].b = 1.0;
276  cols[LIN_SUPN_B_A].b = 1.0;
277  cols[LIN_SUPN_B_B].b = 1.0;
278  cols[LIN_SUPN_L_A].b = 1.0;
279  cols[LIN_SUPN_L_B].b = 1.0;
280 
281  // Footstep line markers
282  FSLines.setType(visualization_msgs::Marker::LINE_LIST);
283  FSLines.marker.points.assign(FIN_COUNT, pt);
284  FSLines.setScale(0.010);
285 
286  // Footstep line markers - Colours
287  visualization_msgs::Marker::_colors_type& fscols = FSLines.marker.colors;
288  fscols.assign(FIN_COUNT, col);
289  fscols[FIN_LFOOTSTEP_F_A].r = 0.6;
290  fscols[FIN_LFOOTSTEP_F_A].g = 0.3;
291  fscols[FIN_LFOOTSTEP_F_B].r = 0.6;
292  fscols[FIN_LFOOTSTEP_F_B].g = 0.3;
293  fscols[FIN_LFOOTSTEP_R_A].r = 0.6;
294  fscols[FIN_LFOOTSTEP_R_A].g = 0.3;
295  fscols[FIN_LFOOTSTEP_R_B].r = 0.6;
296  fscols[FIN_LFOOTSTEP_R_B].g = 0.3;
297  fscols[FIN_LFOOTSTEP_B_A].r = 0.6;
298  fscols[FIN_LFOOTSTEP_B_A].g = 0.3;
299  fscols[FIN_LFOOTSTEP_B_B].r = 0.6;
300  fscols[FIN_LFOOTSTEP_B_B].g = 0.3;
301  fscols[FIN_LFOOTSTEP_L_A].r = 0.6;
302  fscols[FIN_LFOOTSTEP_L_A].g = 0.3;
303  fscols[FIN_LFOOTSTEP_L_B].r = 0.6;
304  fscols[FIN_LFOOTSTEP_L_B].g = 0.3;
305  fscols[FIN_LFOOTSTEP_X_A].r = 0.6;
306  fscols[FIN_LFOOTSTEP_X_A].g = 0.3;
307  fscols[FIN_LFOOTSTEP_X_B].r = 0.6;
308  fscols[FIN_LFOOTSTEP_X_B].g = 0.3;
309  fscols[FIN_LFOOTSTEP_Y_A].r = 0.6;
310  fscols[FIN_LFOOTSTEP_Y_A].g = 0.3;
311  fscols[FIN_LFOOTSTEP_Y_B].r = 0.6;
312  fscols[FIN_LFOOTSTEP_Y_B].g = 0.3;
313  fscols[FIN_RFOOTSTEP_F_A].r = 0.6;
314  fscols[FIN_RFOOTSTEP_F_A].g = 0.3;
315  fscols[FIN_RFOOTSTEP_F_B].r = 0.6;
316  fscols[FIN_RFOOTSTEP_F_B].g = 0.3;
317  fscols[FIN_RFOOTSTEP_R_A].r = 0.6;
318  fscols[FIN_RFOOTSTEP_R_A].g = 0.3;
319  fscols[FIN_RFOOTSTEP_R_B].r = 0.6;
320  fscols[FIN_RFOOTSTEP_R_B].g = 0.3;
321  fscols[FIN_RFOOTSTEP_B_A].r = 0.6;
322  fscols[FIN_RFOOTSTEP_B_A].g = 0.3;
323  fscols[FIN_RFOOTSTEP_B_B].r = 0.6;
324  fscols[FIN_RFOOTSTEP_B_B].g = 0.3;
325  fscols[FIN_RFOOTSTEP_L_A].r = 0.6;
326  fscols[FIN_RFOOTSTEP_L_A].g = 0.3;
327  fscols[FIN_RFOOTSTEP_L_B].r = 0.6;
328  fscols[FIN_RFOOTSTEP_L_B].g = 0.3;
329  fscols[FIN_RFOOTSTEP_X_A].r = 0.6;
330  fscols[FIN_RFOOTSTEP_X_A].g = 0.3;
331  fscols[FIN_RFOOTSTEP_X_B].r = 0.6;
332  fscols[FIN_RFOOTSTEP_X_B].g = 0.3;
333  fscols[FIN_RFOOTSTEP_Y_A].r = 0.6;
334  fscols[FIN_RFOOTSTEP_Y_A].g = 0.3;
335  fscols[FIN_RFOOTSTEP_Y_B].r = 0.6;
336  fscols[FIN_RFOOTSTEP_Y_B].g = 0.3;
337  }
338 
339  // Initialisation function (allowed to assume that model is constructed)
340  void init()
341  {
342  // Do nothing if we don't have a valid RobotModel object
343  if(!model) return;
344  }
345 
346  // Set visualisation offset
347  void setVisOffset(double x, double y, double z)
348  {
349  offsetX = x;
350  offsetY = y;
351  offsetZ = z;
352  }
353 
354  // Update the markers
355  void updateMarkers()
356  {
357  // Do nothing if we don't have a valid RobotModel object
358  if(!model) return;
359 
360  // Retrieve the config parameters
361  const cap_gait::CapConfig* config = model->getConfig();
362  if(!config) return;
363 
364  // Set the model dimensions
365  double JD = config->jointDiameter();
366  Com.setScale(JD);
367  Base.setScale(JD);
368  Head.setScale(config->headDiameter());
369  Trunk.setScale(0.95*JD);
370  Chest.setScale(JD);
371  SuppCom.setScale(0.30*JD, 0.80*JD, 1.00*JD);
372  SuppStep.setScale(0.30*JD, 0.80*JD, 1.00*JD);
373  FreeCom.setScale(0.30*JD, 0.80*JD, 1.00*JD);
374  FreeStep.setScale(0.30*JD, 0.80*JD, 1.00*JD);
375  Joints.setScale(JD);
376  Lines.setScale(0.25*JD);
377  FSLines.setScale(0.25*JD);
378 
379  // Retrieve robot model dimensions
380  double fh = config->footOffsetZ();
381  double fl = config->footLength();
382  double fw = config->footWidth();
383 
384  // Derive robot model dimensions
385  double fwi = 0.50 * fw - config->footOffsetY(); // Distance to outside edge of foot
386  double fwo = 0.50 * fw + config->footOffsetY(); // Distance to inside edge of foot
387  double flb = 0.50 * fl - config->footOffsetX(); // Distance to back edge of foot
388  double flf = 0.50 * fl + config->footOffsetX(); // Distance to front edge of foot
389 
390  // Declare variables
391  visualization_msgs::Marker::_points_type& jpt = Joints.marker.points;
392  visualization_msgs::Marker::_points_type& lpt = Lines.marker.points;
393  visualization_msgs::Marker::_points_type& fpt = FSLines.marker.points;
394 
395  // Retrieve the frame positions
396  qglviewer::Vec comPos = model->com.position();
397  qglviewer::Vec basePos = model->base.position();
398  qglviewer::Vec trunkPos = model->trunkLink.position();
399  qglviewer::Vec neckPos = model->neck.position();
400  qglviewer::Vec headPos = model->head.position();
401  qglviewer::Vec lShoulderPos = model->lShoulder.position();
402  qglviewer::Vec rShoulderPos = model->rShoulder.position();
403  qglviewer::Vec lElbowPos = model->lElbow.position();
404  qglviewer::Vec rElbowPos = model->rElbow.position();
405  qglviewer::Vec lHandPos = model->lHand.position();
406  qglviewer::Vec rHandPos = model->rHand.position();
407  qglviewer::Vec lHipPos = model->lHip.position();
408  qglviewer::Vec rHipPos = model->rHip.position();
409  qglviewer::Vec lKneePos = model->lKnee.position();
410  qglviewer::Vec rKneePos = model->rKnee.position();
411  qglviewer::Vec lAnklePos = model->lAnkle.position();
412  qglviewer::Vec rAnklePos = model->rAnkle.position();
413  qglviewer::Vec lFootPos = model->lFootFloorPoint.position();
414  qglviewer::Vec rFootPos = model->rFootFloorPoint.position();
415  qglviewer::Vec lFootstepPos = model->leftFootstep.position();
416  qglviewer::Vec rFootstepPos = model->rightFootstep.position();
417  qglviewer::Vec sFootstepPos = model->suppFootstep.position();
418  qglviewer::Vec fFootstepPos = model->freeFootstep.position();
419 
420  // Retrieve the frame orientations
421  qglviewer::Quaternion lFootRot = model->lFootFloorPoint.orientation();
422  qglviewer::Quaternion rFootRot = model->rFootFloorPoint.orientation();
423  qglviewer::Quaternion lFootstepRot = model->leftFootstep.orientation();
424  qglviewer::Quaternion rFootstepRot = model->rightFootstep.orientation();
425  qglviewer::Quaternion sFootstepRot = model->suppFootstep.orientation();
426  qglviewer::Quaternion fFootstepRot = model->freeFootstep.orientation();
427 
428  // Retrieve the required robot model information vectors
429  qglviewer::Vec suppComVec = model->suppComVector();
430  qglviewer::Vec suppStepVec = model->suppStepVector();
431  qglviewer::Vec freeComVec = model->freeComVector();
432  qglviewer::Vec freeStepVec = model->freeStepVector();
433 
434  // Offset the frame positions
435  comPos.x += offsetX; comPos.y += offsetY; comPos.z += offsetZ;
436  basePos.x += offsetX; basePos.y += offsetY; basePos.z += offsetZ;
437  headPos.x += offsetX; headPos.y += offsetY; headPos.z += offsetZ;
438  trunkPos.x += offsetX; trunkPos.y += offsetY; trunkPos.z += offsetZ;
439  neckPos.x += offsetX; neckPos.y += offsetY; neckPos.z += offsetZ;
440  lShoulderPos.x += offsetX; lShoulderPos.y += offsetY; lShoulderPos.z += offsetZ;
441  rShoulderPos.x += offsetX; rShoulderPos.y += offsetY; rShoulderPos.z += offsetZ;
442  lElbowPos.x += offsetX; lElbowPos.y += offsetY; lElbowPos.z += offsetZ;
443  rElbowPos.x += offsetX; rElbowPos.y += offsetY; rElbowPos.z += offsetZ;
444  lHandPos.x += offsetX; lHandPos.y += offsetY; lHandPos.z += offsetZ;
445  rHandPos.x += offsetX; rHandPos.y += offsetY; rHandPos.z += offsetZ;
446  lHipPos.x += offsetX; lHipPos.y += offsetY; lHipPos.z += offsetZ;
447  rHipPos.x += offsetX; rHipPos.y += offsetY; rHipPos.z += offsetZ;
448  lKneePos.x += offsetX; lKneePos.y += offsetY; lKneePos.z += offsetZ;
449  rKneePos.x += offsetX; rKneePos.y += offsetY; rKneePos.z += offsetZ;
450  lAnklePos.x += offsetX; lAnklePos.y += offsetY; lAnklePos.z += offsetZ;
451  rAnklePos.x += offsetX; rAnklePos.y += offsetY; rAnklePos.z += offsetZ;
452  lFootPos.x += offsetX; lFootPos.y += offsetY; lFootPos.z += offsetZ;
453  rFootPos.x += offsetX; rFootPos.y += offsetY; rFootPos.z += offsetZ;
454  lFootstepPos.x += offsetX; lFootstepPos.y += offsetY; lFootstepPos.z += offsetZ;
455  rFootstepPos.x += offsetX; rFootstepPos.y += offsetY; rFootstepPos.z += offsetZ;
456  sFootstepPos.x += offsetX; sFootstepPos.y += offsetY; sFootstepPos.z += offsetZ;
457  fFootstepPos.x += offsetX; fFootstepPos.y += offsetY; fFootstepPos.z += offsetZ;
458 
459  // CoM marker
460  Com.update(comPos.x, comPos.y, comPos.z);
461 
462  // Base marker
463  Base.update(basePos.x, basePos.y, basePos.z);
464 
465  // Head marker
466  Head.update(headPos.x, headPos.y, headPos.z);
467 
468  // Trunk marker
469  Trunk.update(trunkPos.x, trunkPos.y, trunkPos.z);
470 
471  // Chest marker
472  Chest.update(0.5*(lShoulderPos.x + rShoulderPos.x), 0.5*(lShoulderPos.y + rShoulderPos.y), 0.5*(lShoulderPos.z + rShoulderPos.z));
473 
474  // Arrow markers
475  if(config->markVectors())
476  {
477  qglviewer::Vec suppComVecTo = sFootstepPos + sFootstepRot * suppComVec;
478  qglviewer::Vec suppStepVecTo = sFootstepPos + sFootstepRot * suppStepVec;
479  qglviewer::Vec freeComVecTo = fFootstepPos + fFootstepRot * freeComVec;
480  qglviewer::Vec freeStepVecTo = fFootstepPos + fFootstepRot * freeStepVec;
481  SuppCom.update(sFootstepPos.x, sFootstepPos.y, sFootstepPos.z, suppComVecTo.x, suppComVecTo.y, suppComVecTo.z);
482  SuppStep.update(sFootstepPos.x, sFootstepPos.y, sFootstepPos.z, suppStepVecTo.x, suppStepVecTo.y, suppStepVecTo.z);
483  FreeCom.update(fFootstepPos.x, fFootstepPos.y, fFootstepPos.z, freeComVecTo.x, freeComVecTo.y, freeComVecTo.z);
484  FreeStep.update(fFootstepPos.x, fFootstepPos.y, fFootstepPos.z, freeStepVecTo.x, freeStepVecTo.y, freeStepVecTo.z);
485  }
486 
487  // Joint markers
488  jpt[JNT_LSHOULDER].x = lShoulderPos.x;
489  jpt[JNT_LSHOULDER].y = lShoulderPos.y;
490  jpt[JNT_LSHOULDER].z = lShoulderPos.z;
491  jpt[JNT_RSHOULDER].x = rShoulderPos.x;
492  jpt[JNT_RSHOULDER].y = rShoulderPos.y;
493  jpt[JNT_RSHOULDER].z = rShoulderPos.z;
494  jpt[JNT_LELBOW].x = lElbowPos.x;
495  jpt[JNT_LELBOW].y = lElbowPos.y;
496  jpt[JNT_LELBOW].z = lElbowPos.z;
497  jpt[JNT_RELBOW].x = rElbowPos.x;
498  jpt[JNT_RELBOW].y = rElbowPos.y;
499  jpt[JNT_RELBOW].z = rElbowPos.z;
500  jpt[JNT_LHIP].x = lHipPos.x;
501  jpt[JNT_LHIP].y = lHipPos.y;
502  jpt[JNT_LHIP].z = lHipPos.z;
503  jpt[JNT_RHIP].x = rHipPos.x;
504  jpt[JNT_RHIP].y = rHipPos.y;
505  jpt[JNT_RHIP].z = rHipPos.z;
506  jpt[JNT_LKNEE].x = lKneePos.x;
507  jpt[JNT_LKNEE].y = lKneePos.y;
508  jpt[JNT_LKNEE].z = lKneePos.z;
509  jpt[JNT_RKNEE].x = rKneePos.x;
510  jpt[JNT_RKNEE].y = rKneePos.y;
511  jpt[JNT_RKNEE].z = rKneePos.z;
512  jpt[JNT_LANKLE].x = lAnklePos.x;
513  jpt[JNT_LANKLE].y = lAnklePos.y;
514  jpt[JNT_LANKLE].z = lAnklePos.z;
515  jpt[JNT_RANKLE].x = rAnklePos.x;
516  jpt[JNT_RANKLE].y = rAnklePos.y;
517  jpt[JNT_RANKLE].z = rAnklePos.z;
518 
519  // Trunk segments (including hip and shoulder lines)
520  lpt[LIN_HIP_A].x = lHipPos.x;
521  lpt[LIN_HIP_A].y = lHipPos.y;
522  lpt[LIN_HIP_A].z = lHipPos.z;
523  lpt[LIN_HIP_B].x = rHipPos.x;
524  lpt[LIN_HIP_B].y = rHipPos.y;
525  lpt[LIN_HIP_B].z = rHipPos.z;
526  lpt[LIN_TRUNK_A].x = basePos.x;
527  lpt[LIN_TRUNK_A].y = basePos.y;
528  lpt[LIN_TRUNK_A].z = basePos.z;
529  lpt[LIN_TRUNK_B].x = neckPos.x;
530  lpt[LIN_TRUNK_B].y = neckPos.y;
531  lpt[LIN_TRUNK_B].z = neckPos.z;
532  lpt[LIN_SHOULDER_A].x = lShoulderPos.x;
533  lpt[LIN_SHOULDER_A].y = lShoulderPos.y;
534  lpt[LIN_SHOULDER_A].z = lShoulderPos.z;
535  lpt[LIN_SHOULDER_B].x = rShoulderPos.x;
536  lpt[LIN_SHOULDER_B].y = rShoulderPos.y;
537  lpt[LIN_SHOULDER_B].z = rShoulderPos.z;
538 
539  // Head segments
540  lpt[LIN_NECK_A].x = neckPos.x;
541  lpt[LIN_NECK_A].y = neckPos.y;
542  lpt[LIN_NECK_A].z = neckPos.z;
543  lpt[LIN_NECK_B].x = headPos.x;
544  lpt[LIN_NECK_B].y = headPos.y;
545  lpt[LIN_NECK_B].z = headPos.z;
546 
547  // Arm segments
548  lpt[LIN_LUARM_A].x = lShoulderPos.x;
549  lpt[LIN_LUARM_A].y = lShoulderPos.y;
550  lpt[LIN_LUARM_A].z = lShoulderPos.z;
551  lpt[LIN_LUARM_B].x = lElbowPos.x;
552  lpt[LIN_LUARM_B].y = lElbowPos.y;
553  lpt[LIN_LUARM_B].z = lElbowPos.z;
554  lpt[LIN_LLARM_A].x = lElbowPos.x;
555  lpt[LIN_LLARM_A].y = lElbowPos.y;
556  lpt[LIN_LLARM_A].z = lElbowPos.z;
557  lpt[LIN_LLARM_B].x = lHandPos.x;
558  lpt[LIN_LLARM_B].y = lHandPos.y;
559  lpt[LIN_LLARM_B].z = lHandPos.z;
560  lpt[LIN_RUARM_A].x = rShoulderPos.x;
561  lpt[LIN_RUARM_A].y = rShoulderPos.y;
562  lpt[LIN_RUARM_A].z = rShoulderPos.z;
563  lpt[LIN_RUARM_B].x = rElbowPos.x;
564  lpt[LIN_RUARM_B].y = rElbowPos.y;
565  lpt[LIN_RUARM_B].z = rElbowPos.z;
566  lpt[LIN_RLARM_A].x = rElbowPos.x;
567  lpt[LIN_RLARM_A].y = rElbowPos.y;
568  lpt[LIN_RLARM_A].z = rElbowPos.z;
569  lpt[LIN_RLARM_B].x = rHandPos.x;
570  lpt[LIN_RLARM_B].y = rHandPos.y;
571  lpt[LIN_RLARM_B].z = rHandPos.z;
572 
573  // Leg segments
574  lpt[LIN_LTHIGH_A].x = lHipPos.x;
575  lpt[LIN_LTHIGH_A].y = lHipPos.y;
576  lpt[LIN_LTHIGH_A].z = lHipPos.z;
577  lpt[LIN_LTHIGH_B].x = lKneePos.x;
578  lpt[LIN_LTHIGH_B].y = lKneePos.y;
579  lpt[LIN_LTHIGH_B].z = lKneePos.z;
580  lpt[LIN_RTHIGH_A].x = rHipPos.x;
581  lpt[LIN_RTHIGH_A].y = rHipPos.y;
582  lpt[LIN_RTHIGH_A].z = rHipPos.z;
583  lpt[LIN_RTHIGH_B].x = rKneePos.x;
584  lpt[LIN_RTHIGH_B].y = rKneePos.y;
585  lpt[LIN_RTHIGH_B].z = rKneePos.z;
586  lpt[LIN_LSHANK_A].x = lKneePos.x;
587  lpt[LIN_LSHANK_A].y = lKneePos.y;
588  lpt[LIN_LSHANK_A].z = lKneePos.z;
589  lpt[LIN_LSHANK_B].x = lAnklePos.x;
590  lpt[LIN_LSHANK_B].y = lAnklePos.y;
591  lpt[LIN_LSHANK_B].z = lAnklePos.z;
592  lpt[LIN_RSHANK_A].x = rKneePos.x;
593  lpt[LIN_RSHANK_A].y = rKneePos.y;
594  lpt[LIN_RSHANK_A].z = rKneePos.z;
595  lpt[LIN_RSHANK_B].x = rAnklePos.x;
596  lpt[LIN_RSHANK_B].y = rAnklePos.y;
597  lpt[LIN_RSHANK_B].z = rAnklePos.z;
598 
599  // Left foot
600  float lMat[3][3] = {{0}};
601  lFootRot.getRotationMatrix(lMat);
602  lpt[LIN_LFOOTX_A].x = lFootPos.x - flb * lMat[0][0];
603  lpt[LIN_LFOOTX_A].y = lFootPos.y - flb * lMat[1][0];
604  lpt[LIN_LFOOTX_A].z = lFootPos.z - flb * lMat[2][0];
605  lpt[LIN_LFOOTX_B].x = lFootPos.x + flf * lMat[0][0];
606  lpt[LIN_LFOOTX_B].y = lFootPos.y + flf * lMat[1][0];
607  lpt[LIN_LFOOTX_B].z = lFootPos.z + flf * lMat[2][0];
608  lpt[LIN_LFOOTY_A].x = lFootPos.x - fwi * lMat[0][1];
609  lpt[LIN_LFOOTY_A].y = lFootPos.y - fwi * lMat[1][1];
610  lpt[LIN_LFOOTY_A].z = lFootPos.z - fwi * lMat[2][1];
611  lpt[LIN_LFOOTY_B].x = lFootPos.x + fwo * lMat[0][1];
612  lpt[LIN_LFOOTY_B].y = lFootPos.y + fwo * lMat[1][1];
613  lpt[LIN_LFOOTY_B].z = lFootPos.z + fwo * lMat[2][1];
614  lpt[LIN_LFOOTZ_A].x = lFootPos.x;
615  lpt[LIN_LFOOTZ_A].y = lFootPos.y;
616  lpt[LIN_LFOOTZ_A].z = lFootPos.z;
617  lpt[LIN_LFOOTZ_B].x = lFootPos.x + fh * lMat[0][2];
618  lpt[LIN_LFOOTZ_B].y = lFootPos.y + fh * lMat[1][2];
619  lpt[LIN_LFOOTZ_B].z = lFootPos.z + fh * lMat[2][2];
620 
621  // Right foot
622  float rMat[3][3] = {{0}};
623  rFootRot.getRotationMatrix(rMat);
624  lpt[LIN_RFOOTX_A].x = rFootPos.x - flb * rMat[0][0];
625  lpt[LIN_RFOOTX_A].y = rFootPos.y - flb * rMat[1][0];
626  lpt[LIN_RFOOTX_A].z = rFootPos.z - flb * rMat[2][0];
627  lpt[LIN_RFOOTX_B].x = rFootPos.x + flf * rMat[0][0];
628  lpt[LIN_RFOOTX_B].y = rFootPos.y + flf * rMat[1][0];
629  lpt[LIN_RFOOTX_B].z = rFootPos.z + flf * rMat[2][0];
630  lpt[LIN_RFOOTY_A].x = rFootPos.x - fwo * rMat[0][1];
631  lpt[LIN_RFOOTY_A].y = rFootPos.y - fwo * rMat[1][1];
632  lpt[LIN_RFOOTY_A].z = rFootPos.z - fwo * rMat[2][1];
633  lpt[LIN_RFOOTY_B].x = rFootPos.x + fwi * rMat[0][1];
634  lpt[LIN_RFOOTY_B].y = rFootPos.y + fwi * rMat[1][1];
635  lpt[LIN_RFOOTY_B].z = rFootPos.z + fwi * rMat[2][1];
636  lpt[LIN_RFOOTZ_A].x = rFootPos.x;
637  lpt[LIN_RFOOTZ_A].y = rFootPos.y;
638  lpt[LIN_RFOOTZ_A].z = rFootPos.z;
639  lpt[LIN_RFOOTZ_B].x = rFootPos.x + fh * rMat[0][2];
640  lpt[LIN_RFOOTZ_B].y = rFootPos.y + fh * rMat[1][2];
641  lpt[LIN_RFOOTZ_B].z = rFootPos.z + fh * rMat[2][2];
642 
643  // Foot boundaries
644  double fwp = 0.0, fwn = 0.0;
645  qglviewer::Vec* suppFootPos;
646  qglviewer::Vec* supnFootPos;
647  float (*suppFootMat)[3][3];
648  float (*supnFootMat)[3][3];
649  if(model->supportLegSign > 0) // Support foot is right foot
650  {
651  suppFootPos = &rFootPos;
652  supnFootPos = &lFootPos;
653  suppFootMat = &rMat;
654  supnFootMat = &lMat;
655  fwp = fwi;
656  fwn = fwo;
657  }
658  else // Support foot is left foot
659  {
660  suppFootPos = &lFootPos;
661  supnFootPos = &rFootPos;
662  suppFootMat = &lMat;
663  supnFootMat = &rMat;
664  fwp = fwo;
665  fwn = fwi;
666  }
667  lpt[LIN_SUPP_F_A].x = suppFootPos->x + flf * (*suppFootMat)[0][0] - fwn * (*suppFootMat)[0][1];
668  lpt[LIN_SUPP_F_A].y = suppFootPos->y + flf * (*suppFootMat)[1][0] - fwn * (*suppFootMat)[1][1];
669  lpt[LIN_SUPP_F_A].z = suppFootPos->z + flf * (*suppFootMat)[2][0] - fwn * (*suppFootMat)[2][1];
670  lpt[LIN_SUPP_R_A].x = suppFootPos->x + flf * (*suppFootMat)[0][0] + fwp * (*suppFootMat)[0][1];
671  lpt[LIN_SUPP_R_A].y = suppFootPos->y + flf * (*suppFootMat)[1][0] + fwp * (*suppFootMat)[1][1];
672  lpt[LIN_SUPP_R_A].z = suppFootPos->z + flf * (*suppFootMat)[2][0] + fwp * (*suppFootMat)[2][1];
673  lpt[LIN_SUPP_B_A].x = suppFootPos->x - flb * (*suppFootMat)[0][0] + fwp * (*suppFootMat)[0][1];
674  lpt[LIN_SUPP_B_A].y = suppFootPos->y - flb * (*suppFootMat)[1][0] + fwp * (*suppFootMat)[1][1];
675  lpt[LIN_SUPP_B_A].z = suppFootPos->z - flb * (*suppFootMat)[2][0] + fwp * (*suppFootMat)[2][1];
676  lpt[LIN_SUPP_L_A].x = suppFootPos->x - flb * (*suppFootMat)[0][0] - fwn * (*suppFootMat)[0][1];
677  lpt[LIN_SUPP_L_A].y = suppFootPos->y - flb * (*suppFootMat)[1][0] - fwn * (*suppFootMat)[1][1];
678  lpt[LIN_SUPP_L_A].z = suppFootPos->z - flb * (*suppFootMat)[2][0] - fwn * (*suppFootMat)[2][1];
679  lpt[LIN_SUPP_F_B] = lpt[LIN_SUPP_R_A];
680  lpt[LIN_SUPP_R_B] = lpt[LIN_SUPP_B_A];
681  lpt[LIN_SUPP_B_B] = lpt[LIN_SUPP_L_A];
682  lpt[LIN_SUPP_L_B] = lpt[LIN_SUPP_F_A];
683  lpt[LIN_SUPN_F_A].x = supnFootPos->x + flf * (*supnFootMat)[0][0] - fwp * (*supnFootMat)[0][1];
684  lpt[LIN_SUPN_F_A].y = supnFootPos->y + flf * (*supnFootMat)[1][0] - fwp * (*supnFootMat)[1][1];
685  lpt[LIN_SUPN_F_A].z = supnFootPos->z + flf * (*supnFootMat)[2][0] - fwp * (*supnFootMat)[2][1];
686  lpt[LIN_SUPN_R_A].x = supnFootPos->x + flf * (*supnFootMat)[0][0] + fwn * (*supnFootMat)[0][1];
687  lpt[LIN_SUPN_R_A].y = supnFootPos->y + flf * (*supnFootMat)[1][0] + fwn * (*supnFootMat)[1][1];
688  lpt[LIN_SUPN_R_A].z = supnFootPos->z + flf * (*supnFootMat)[2][0] + fwn * (*supnFootMat)[2][1];
689  lpt[LIN_SUPN_B_A].x = supnFootPos->x - flb * (*supnFootMat)[0][0] + fwn * (*supnFootMat)[0][1];
690  lpt[LIN_SUPN_B_A].y = supnFootPos->y - flb * (*supnFootMat)[1][0] + fwn * (*supnFootMat)[1][1];
691  lpt[LIN_SUPN_B_A].z = supnFootPos->z - flb * (*supnFootMat)[2][0] + fwn * (*supnFootMat)[2][1];
692  lpt[LIN_SUPN_L_A].x = supnFootPos->x - flb * (*supnFootMat)[0][0] - fwp * (*supnFootMat)[0][1];
693  lpt[LIN_SUPN_L_A].y = supnFootPos->y - flb * (*supnFootMat)[1][0] - fwp * (*supnFootMat)[1][1];
694  lpt[LIN_SUPN_L_A].z = supnFootPos->z - flb * (*supnFootMat)[2][0] - fwp * (*supnFootMat)[2][1];
695  lpt[LIN_SUPN_F_B] = lpt[LIN_SUPN_R_A];
696  lpt[LIN_SUPN_R_B] = lpt[LIN_SUPN_B_A];
697  lpt[LIN_SUPN_B_B] = lpt[LIN_SUPN_L_A];
698  lpt[LIN_SUPN_L_B] = lpt[LIN_SUPN_F_A];
699 
700  // Left footstep
701  float lFSMat[3][3] = {{0}};
702  lFootstepRot.getRotationMatrix(lFSMat);
703  fpt[FIN_LFOOTSTEP_X_A].x = lFootstepPos.x - flb * lFSMat[0][0];
704  fpt[FIN_LFOOTSTEP_X_A].y = lFootstepPos.y - flb * lFSMat[1][0];
705  fpt[FIN_LFOOTSTEP_X_A].z = lFootstepPos.z - flb * lFSMat[2][0];
706  fpt[FIN_LFOOTSTEP_X_B].x = lFootstepPos.x + flf * lFSMat[0][0];
707  fpt[FIN_LFOOTSTEP_X_B].y = lFootstepPos.y + flf * lFSMat[1][0];
708  fpt[FIN_LFOOTSTEP_X_B].z = lFootstepPos.z + flf * lFSMat[2][0];
709  fpt[FIN_LFOOTSTEP_Y_A].x = lFootstepPos.x - fwi * lFSMat[0][1];
710  fpt[FIN_LFOOTSTEP_Y_A].y = lFootstepPos.y - fwi * lFSMat[1][1];
711  fpt[FIN_LFOOTSTEP_Y_A].z = lFootstepPos.z - fwi * lFSMat[2][1];
712  fpt[FIN_LFOOTSTEP_Y_B].x = lFootstepPos.x + fwo * lFSMat[0][1];
713  fpt[FIN_LFOOTSTEP_Y_B].y = lFootstepPos.y + fwo * lFSMat[1][1];
714  fpt[FIN_LFOOTSTEP_Y_B].z = lFootstepPos.z + fwo * lFSMat[2][1];
715  fpt[FIN_LFOOTSTEP_F_A].x = lFootstepPos.x + flf * lFSMat[0][0] - fwi * lFSMat[0][1];
716  fpt[FIN_LFOOTSTEP_F_A].y = lFootstepPos.y + flf * lFSMat[1][0] - fwi * lFSMat[1][1];
717  fpt[FIN_LFOOTSTEP_F_A].z = lFootstepPos.z + flf * lFSMat[2][0] - fwi * lFSMat[2][1];
718  fpt[FIN_LFOOTSTEP_R_A].x = lFootstepPos.x + flf * lFSMat[0][0] + fwo * lFSMat[0][1];
719  fpt[FIN_LFOOTSTEP_R_A].y = lFootstepPos.y + flf * lFSMat[1][0] + fwo * lFSMat[1][1];
720  fpt[FIN_LFOOTSTEP_R_A].z = lFootstepPos.z + flf * lFSMat[2][0] + fwo * lFSMat[2][1];
721  fpt[FIN_LFOOTSTEP_B_A].x = lFootstepPos.x - flb * lFSMat[0][0] + fwo * lFSMat[0][1];
722  fpt[FIN_LFOOTSTEP_B_A].y = lFootstepPos.y - flb * lFSMat[1][0] + fwo * lFSMat[1][1];
723  fpt[FIN_LFOOTSTEP_B_A].z = lFootstepPos.z - flb * lFSMat[2][0] + fwo * lFSMat[2][1];
724  fpt[FIN_LFOOTSTEP_L_A].x = lFootstepPos.x - flb * lFSMat[0][0] - fwi * lFSMat[0][1];
725  fpt[FIN_LFOOTSTEP_L_A].y = lFootstepPos.y - flb * lFSMat[1][0] - fwi * lFSMat[1][1];
726  fpt[FIN_LFOOTSTEP_L_A].z = lFootstepPos.z - flb * lFSMat[2][0] - fwi * lFSMat[2][1];
727  fpt[FIN_LFOOTSTEP_F_B] = fpt[FIN_LFOOTSTEP_R_A];
728  fpt[FIN_LFOOTSTEP_R_B] = fpt[FIN_LFOOTSTEP_B_A];
729  fpt[FIN_LFOOTSTEP_B_B] = fpt[FIN_LFOOTSTEP_L_A];
730  fpt[FIN_LFOOTSTEP_L_B] = fpt[FIN_LFOOTSTEP_F_A];
731 
732  // Right footstep
733  float rFSMat[3][3] = {{0}};
734  rFootstepRot.getRotationMatrix(rFSMat);
735  fpt[FIN_RFOOTSTEP_X_A].x = rFootstepPos.x - flb * rFSMat[0][0];
736  fpt[FIN_RFOOTSTEP_X_A].y = rFootstepPos.y - flb * rFSMat[1][0];
737  fpt[FIN_RFOOTSTEP_X_A].z = rFootstepPos.z - flb * rFSMat[2][0];
738  fpt[FIN_RFOOTSTEP_X_B].x = rFootstepPos.x + flf * rFSMat[0][0];
739  fpt[FIN_RFOOTSTEP_X_B].y = rFootstepPos.y + flf * rFSMat[1][0];
740  fpt[FIN_RFOOTSTEP_X_B].z = rFootstepPos.z + flf * rFSMat[2][0];
741  fpt[FIN_RFOOTSTEP_Y_A].x = rFootstepPos.x - fwo * rFSMat[0][1];
742  fpt[FIN_RFOOTSTEP_Y_A].y = rFootstepPos.y - fwo * rFSMat[1][1];
743  fpt[FIN_RFOOTSTEP_Y_A].z = rFootstepPos.z - fwo * rFSMat[2][1];
744  fpt[FIN_RFOOTSTEP_Y_B].x = rFootstepPos.x + fwi * rFSMat[0][1];
745  fpt[FIN_RFOOTSTEP_Y_B].y = rFootstepPos.y + fwi * rFSMat[1][1];
746  fpt[FIN_RFOOTSTEP_Y_B].z = rFootstepPos.z + fwi * rFSMat[2][1];
747  fpt[FIN_RFOOTSTEP_F_A].x = rFootstepPos.x + flf * rFSMat[0][0] - fwo * rFSMat[0][1];
748  fpt[FIN_RFOOTSTEP_F_A].y = rFootstepPos.y + flf * rFSMat[1][0] - fwo * rFSMat[1][1];
749  fpt[FIN_RFOOTSTEP_F_A].z = rFootstepPos.z + flf * rFSMat[2][0] - fwo * rFSMat[2][1];
750  fpt[FIN_RFOOTSTEP_R_A].x = rFootstepPos.x + flf * rFSMat[0][0] + fwi * rFSMat[0][1];
751  fpt[FIN_RFOOTSTEP_R_A].y = rFootstepPos.y + flf * rFSMat[1][0] + fwi * rFSMat[1][1];
752  fpt[FIN_RFOOTSTEP_R_A].z = rFootstepPos.z + flf * rFSMat[2][0] + fwi * rFSMat[2][1];
753  fpt[FIN_RFOOTSTEP_B_A].x = rFootstepPos.x - flb * rFSMat[0][0] + fwi * rFSMat[0][1];
754  fpt[FIN_RFOOTSTEP_B_A].y = rFootstepPos.y - flb * rFSMat[1][0] + fwi * rFSMat[1][1];
755  fpt[FIN_RFOOTSTEP_B_A].z = rFootstepPos.z - flb * rFSMat[2][0] + fwi * rFSMat[2][1];
756  fpt[FIN_RFOOTSTEP_L_A].x = rFootstepPos.x - flb * rFSMat[0][0] - fwo * rFSMat[0][1];
757  fpt[FIN_RFOOTSTEP_L_A].y = rFootstepPos.y - flb * rFSMat[1][0] - fwo * rFSMat[1][1];
758  fpt[FIN_RFOOTSTEP_L_A].z = rFootstepPos.z - flb * rFSMat[2][0] - fwo * rFSMat[2][1];
759  fpt[FIN_RFOOTSTEP_F_B] = fpt[FIN_RFOOTSTEP_R_A];
760  fpt[FIN_RFOOTSTEP_R_B] = fpt[FIN_RFOOTSTEP_B_A];
761  fpt[FIN_RFOOTSTEP_B_B] = fpt[FIN_RFOOTSTEP_L_A];
762  fpt[FIN_RFOOTSTEP_L_B] = fpt[FIN_RFOOTSTEP_F_A];
763 
764  // Manually add the generic markers (GenMarker) for publishing
765  add(&Joints);
766  add(&Lines);
767  add(&FSLines);
768  }
769 
770  // RobotModel object
771  const RobotModel* model;
772 
773  // Reference frame for visualisation
774  const std::string frame;
775 
776  private:
777  // Markers
778  vis_utils::SphereMarker Com;
779  vis_utils::SphereMarker Base;
780  vis_utils::SphereMarker Head;
781  vis_utils::SphereMarker Trunk;
782  vis_utils::SphereMarker Chest;
783  vis_utils::ArrowMarker SuppCom;
784  vis_utils::ArrowMarker SuppStep;
785  vis_utils::ArrowMarker FreeCom;
786  vis_utils::ArrowMarker FreeStep;
787  vis_utils::GenMarker Joints;
788  vis_utils::GenMarker Lines;
789  vis_utils::GenMarker FSLines;
790 
791  // Visualisation offsets
792  double offsetX;
793  double offsetY;
794  double offsetZ;
795 
796  };
797 }
798 
799 #endif /* ROBOTMODELVIS_H */
800 // EOF
cap_gait::CapConfig::markVectors
config_server::Parameter< bool > markVectors
Boolean flag whether to publish arrow markers showing the various vector quantities of the robot mode...
Definition: cap_gait_config.h:404
cap_gait::CapConfig::footOffsetX
config_server::Parameter< float > footOffsetX
Backward offset from the foot plate geometric center to the ankle joint (along x-axis) ...
Definition: cap_gait_config.h:387
cap_gait::CapConfig::footOffsetY
config_server::Parameter< float > footOffsetY
Inward offset from the foot plate geometric center to the ankle joint (along y-axis) ...
Definition: cap_gait_config.h:388
cap_gait::CapConfig::headDiameter
config_server::Parameter< float > headDiameter
Approximate diameter of the head (not for analytic calculation)
Definition: cap_gait_config.h:399
cap_gait::CapConfig::footWidth
config_server::Parameter< float > footWidth
Width of the robot foot (along y-axis, the foot plate is assumed to be rectangular) ...
Definition: cap_gait_config.h:385
cap_gait::CapConfig::footLength
config_server::Parameter< float > footLength
Length of the robot foot (along x-axis, the foot plate is assumed to be rectangular) ...
Definition: cap_gait_config.h:386
margait_contrib::RobotModelVis
Encapsulates a visualisation of the state of a RobotModel object.
Definition: RobotModelVis.h:29
cap_gait::CapConfig
Configuration struct for the capture step gait.
Definition: cap_gait_config.h:21
cap_gait::CapConfig::footOffsetZ
config_server::Parameter< float > footOffsetZ
Upward offset from the foot plate geometric center to the ankle joint (along z-axis) ...
Definition: cap_gait_config.h:389
cap_gait::CapConfig::jointDiameter
config_server::Parameter< float > jointDiameter
Diameter of the joints (not for analytic calculation)
Definition: cap_gait_config.h:400