rviz_robot_builder.cc
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2017, Alexander W. Winkler. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6 
7 * Redistributions of source code must retain the above copyright notice, this
8  list of conditions and the following disclaimer.
9 
10 * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13 
14 * Neither the name of the copyright holder nor the names of its
15  contributors may be used to endorse or promote products derived from
16  this software without specific prior written permission.
17 
18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ******************************************************************************/
29 
31 
32 #include <xpp_states/convert.h>
33 #include <xpp_vis/rviz_colors.h>
34 
35 namespace xpp {
36 
38 {
39  terrain_msg_.friction_coeff = 0.0;
40 }
41 
42 void
43 RvizRobotBuilder::SetRobotParameters (const xpp_msgs::RobotParameters& msg)
44 {
45  params_msg_ = msg;
46 }
47 
48 void
49 RvizRobotBuilder::SetTerrainParameters (const xpp_msgs::TerrainInfo& msg)
50 {
51  terrain_msg_ = msg;
52 }
53 
54 void
56 {
57  Marker invisible = vec.empty()? Marker() : vec.back();
58  invisible.color.a = 0.0;
59  vec.resize(max_size, invisible);
60 }
61 
63 RvizRobotBuilder::BuildRobotState (const xpp_msgs::RobotStateCartesian& state_msg) const
64 {
65  MarkerArray msg;
66 
67  auto state = Convert::ToXpp(state_msg);
68  auto ee_pos = state.ee_motion_.Get(kPos);
69 
70  Marker base = CreateBasePose(state.base_.lin.p_,
71  state.base_.ang.q,
72  state.ee_contact_);
73  msg.markers.push_back(base);
74 
75  MarkerVec m_ee_pos = CreateEEPositions(ee_pos, state.ee_contact_);
76  FillWithInvisible(max_ee_, m_ee_pos);
77  msg.markers.insert(msg.markers.begin(), m_ee_pos.begin(), m_ee_pos.end());
78 
79  MarkerVec ee_forces = CreateEEForces(state.ee_forces_, ee_pos, state.ee_contact_);
80  FillWithInvisible(max_ee_, ee_forces);
81  msg.markers.insert(msg.markers.begin(), ee_forces.begin(), ee_forces.end());
82 
83  MarkerVec rom = CreateRangeOfMotion(state.base_);
85  msg.markers.insert(msg.markers.begin(), rom.begin(), rom.end());
86 
87  MarkerVec support = CreateSupportArea(state.ee_contact_, ee_pos);
88  FillWithInvisible(max_ee_, support);
89  msg.markers.insert(msg.markers.begin(), support.begin(), support.end());
90 
91  MarkerVec friction = CreateFrictionCones(ee_pos, state.ee_contact_);
92  FillWithInvisible(max_ee_, friction);
93  msg.markers.insert(msg.markers.begin(), friction.begin(), friction.end());
94 
95  Marker cop = CreateCopPos(state.ee_forces_, ee_pos);
96  msg.markers.push_back(cop);
97 
98  Marker ip = CreatePendulum(state.base_.lin.p_, state.ee_forces_, ee_pos);
99  msg.markers.push_back(ip);
100 
101  msg.markers.push_back(CreateGravityForce(state.base_.lin.p_));
102 
103  int id = 0;
104  for (Marker& m : msg.markers) {
105  m.header.frame_id = frame_id_;
106  m.id = id++;
107  }
108 
109  return msg;
110 }
111 
114  const ContactState& in_contact) const
115 {
116  MarkerVec vec;
117 
118  for (auto ee : ee_pos.GetEEsOrdered()) {
119  Marker m = CreateSphere(ee_pos.at(ee), 0.04);
120  m.ns = "endeffector_pos";
121  m.color = color.blue;
122 
123  vec.push_back(m);
124  }
125 
126  return vec;
127 }
128 
131 {
132  double g = 9.81;
133  double mass = params_msg_.base_mass;
134  Marker m = CreateForceArrow(Eigen::Vector3d(0.0, 0.0, -mass*g), base_pos);
135  m.color = color.red;
136  m.ns = "gravity_force";
137 
138  return m;
139 }
140 
143  const EEPos& ee_pos,
144  const ContactState& contact_state) const
145 {
146  MarkerVec vec;
147 
148  for (auto ee : ee_forces.GetEEsOrdered()) {
149  Vector3d p = ee_pos.at(ee);
150  Vector3d f = ee_forces.at(ee);
151 
152  Marker m = CreateForceArrow(f, p);
153  m.color = color.red;
154  m.color.a = f.norm() > 0.1 ? 1.0 : 0.0;
155  m.ns = "ee_force";
156  vec.push_back(m);
157  }
158 
159  return vec;
160 }
161 
164  const ContactState& contact_state) const
165 {
166  MarkerVec vec;
167 
168  // add at least one invisible to have namespace template to fill
169  Marker invisible = CreateFrictionCone(Vector3d::Zero(), Vector3d::Zero(), 0.0);
170  invisible.color.a = 0.0;
171  vec.push_back(invisible);
172 
173  // only draw cones if terrain_msg and robot state correspond
174  double mu = terrain_msg_.friction_coeff;
175  if (ee_pos.GetEECount() == terrain_msg_.surface_normals.size() && mu > 1e-3) {
176 
177  auto normals = Convert::ToXpp(terrain_msg_.surface_normals);
178  for (auto ee : normals.GetEEsOrdered()) {
179  Marker m;
180  Vector3d n = normals.at(ee);
181  m = CreateFrictionCone(ee_pos.at(ee), -n, mu);
182  m.color = color.red;
183  m.color.a = contact_state.at(ee)? 0.25 : 0.0;
184  vec.push_back(m);
185  }
186  }
187 
188  return vec;
189 }
190 
193  const Vector3d& normal,
194  double mu) const
195 {
196  Marker m;
197  m.type = Marker::ARROW;
198  m.ns = "friction_cone";
199 
200  double cone_height = 0.1; // [m]
201 
202  m.scale.x = 0.00; // [shaft diameter] hide arrow shaft to generate cone
203  m.scale.y = 2.0 * cone_height * mu; // arrow-head diameter
204  m.scale.z = cone_height; // arrow head length
205 
206  auto start = Convert::ToRos<geometry_msgs::Point>(pos + normal);
207  m.points.push_back(start);
208 
209  auto end = Convert::ToRos<geometry_msgs::Point>(pos);
210  m.points.push_back(end);
211 
212  return m;
213 }
214 
217  Eigen::Quaterniond ori,
218  const ContactState& contact_state) const
219 {
220  Vector3d edge_length(0.1, 0.05, 0.02);
221  Marker m = CreateBox(pos, ori, 3*edge_length);
222 
223  m.color = color.black;
224  m.color.a = 0.8;
225  for (auto ee : contact_state.GetEEsOrdered())
226  if (contact_state.at(ee))
227  m.color = color.black;
228 
229  m.ns = "base_pose";
230 
231  return m;
232 }
233 
236  const EEPos& ee_pos) const
237 {
238  double z_sum = 0.0;
239  for (Vector3d ee : ee_forces.ToImpl())
240  z_sum += ee.z();
241 
242  Marker m;
243  // only then can the Center of Pressure be calculated
244  Vector3d cop = Vector3d::Zero();
245  if (z_sum > 0.0) {
246  for (auto ee : ee_forces.GetEEsOrdered()) {
247  double p = ee_forces.at(ee).z()/z_sum;
248  cop += p*ee_pos.at(ee);
249  }
250  m = CreateSphere(cop);
251  } else {
252  m = CreateSphere(cop, 0.001); // no CoP exists b/c flight phase
253  }
254 
255  m.color = color.red;
256  m.ns = "cop";
257 
258  return m;
259 }
260 
263  const EEForces& ee_forces,
264  const EEPos& ee_pos) const
265 {
266  Marker m;
267  m.type = Marker::LINE_STRIP;
268  m.scale.x = 0.007; // thickness of pendulum pole
269 
270  geometry_msgs::Point cop = CreateCopPos(ee_forces, ee_pos).pose.position;
271  geometry_msgs::Point com = CreateSphere(pos).pose.position;
272 
273  m.points.push_back(cop);
274  m.points.push_back(com);
275 
276  m.ns = "inverted_pendulum";
277  m.color = color.black;
278 
279  double fz_sum = 0.0;
280  for (Vector3d ee : ee_forces.ToImpl())
281  fz_sum += ee.z();
282 
283  if (fz_sum < 1.0) // [N] flight phase
284  m.color.a = 0.0; // hide marker
285 
286  return m;
287 }
288 
291 {
292  MarkerVec vec;
293 
294  auto w_R_b = base.ang.q.toRotationMatrix();
295 
296  for (const auto& pos_B : params_msg_.nominal_ee_pos) {
297  Vector3d pos_W = base.lin.p_ + w_R_b*Convert::ToXpp(pos_B);
298 
299  Vector3d edge_length = 2*Convert::ToXpp(params_msg_.ee_max_dev);
300  Marker m = CreateBox(pos_W, base.ang.q, edge_length);
301  m.color = color.blue;
302  m.color.a = 0.2;
303  m.ns = "range_of_motion";
304  vec.push_back(m);
305  }
306 
307  return vec;
308 }
309 
311 RvizRobotBuilder::CreateBox (const Vector3d& pos, Eigen::Quaterniond ori,
312  const Vector3d& edge_length) const
313 {
314  Marker m;
315 
316  m.type = Marker::CUBE;
317  m.pose.position = Convert::ToRos<geometry_msgs::Point>(pos);
318  m.pose.orientation = Convert::ToRos(ori);
319  m.scale = Convert::ToRos<geometry_msgs::Vector3>(edge_length);
320 
321  return m;
322 }
323 
325 RvizRobotBuilder::CreateSphere (const Vector3d& pos, double diameter) const
326 {
327  Marker m;
328 
329  m.type = Marker::SPHERE;
330  m.pose.position = Convert::ToRos<geometry_msgs::Point>(pos);
331  m.scale.x = diameter;
332  m.scale.y = diameter;
333  m.scale.z = diameter;
334 
335  return m;
336 }
337 
340  const Vector3d& ee_pos) const
341 {
342  Marker m;
343  m.type = Marker::ARROW;
344 
345  m.scale.x = 0.01; // shaft diameter
346  m.scale.y = 0.02; // arrow-head diameter
347  m.scale.z = 0.06; // arrow-head length
348 
349  double force_scale = 800;
350  Vector3d p_start = ee_pos - force/force_scale;
351  auto start = Convert::ToRos<geometry_msgs::Point>(p_start);
352  m.points.push_back(start);
353 
354  auto end = Convert::ToRos<geometry_msgs::Point>(ee_pos);
355  m.points.push_back(end);
356 
357  return m;
358 }
359 
362  const EEPos& ee_pos) const
363 {
364  MarkerVec vec;
365 
366  Marker m;
367  m.ns = "support_polygons";
368  m.scale.x = m.scale.y = m.scale.z = 1.0;
369 
370  for (auto ee : contact_state.GetEEsOrdered()) {
371  if (contact_state.at(ee)) { // endeffector in contact
372  auto p = Convert::ToRos<geometry_msgs::Point>(ee_pos.at(ee));
373  m.points.push_back(p);
374  m.color = color.black;
375  m.color.a = 0.2;
376  }
377  }
378 
379  switch (m.points.size()) {
380  case 4: {
381  m.type = Marker::TRIANGLE_LIST;
382  auto temp = m.points;
383 
384  // add two triangles to represent a square
385  m.points.pop_back();
386  vec.push_back(m);
387 
388  m.points = temp;
389  m.points.erase(m.points.begin());
390  vec.push_back(m);
391  break;
392  }
393  case 3: {
394  m.type = Marker::TRIANGLE_LIST;
395  vec.push_back(m);
396  vec.push_back(m);
397  break;
398  }
399  case 2: {
400  m.type = Marker::LINE_STRIP;
401  m.scale.x = 0.01;
402  vec.push_back(m);
403  vec.push_back(m);
404  break;
405  }
406  case 1: {
407  /* just make so small that random marker can't be seen */
408  m.type = Marker::CUBE; // just so same shape is specified
409  m.color.a = 0.0; // hide marker
410  vec.push_back(m);
411  vec.push_back(m);
412  break;
413  }
414  default:
415  m.type = Marker::CUBE; // just so same shapes is specified
416  m.color.a = 0.0; // hide marker
417  vec.push_back(m);
418  vec.push_back(m);
419  break;
420  }
421 
422  return vec;
423 }
424 
425 } /* namespace xpp */
int GetEECount() const
Quaterniond q
std::vector< EndeffectorID > GetEEsOrdered() const
static xpp_msgs::StateLin3d ToRos(const StateLin3d &xpp)
ROSCPP_DECL void start()
T & at(EndeffectorID ee)
f
MarkerVec CreateFrictionCones(const EEPos &pos_W, const ContactState &c) const
void SetTerrainParameters(const xpp_msgs::TerrainInfo &msg)
Additional information that can be used for visualization.
MarkerVec CreateEEPositions(const EEPos &pos_W, const ContactState &c) const
static ColorT color
Definition: rviz_colors.h:69
void FillWithInvisible(int max_size, MarkerVec &vec) const
xpp_msgs::RobotParameters params_msg_
MarkerArray BuildRobotState(const xpp_msgs::RobotStateCartesian &msg) const
Constructs the RVIZ markers from the ROS message.
MarkerVec CreateRangeOfMotion(const State3d &base) const
void SetRobotParameters(const xpp_msgs::RobotParameters &msg)
Provides additional robot info that can be used for visualization.
Eigen::Vector3d Vector3d
Marker CreateFrictionCone(const Vector3d &pos_W, const Vector3d &terrain_normal, double friction_coeff) const
Marker CreateBasePose(const Vector3d &pos, Eigen::Quaterniond ori, const ContactState &c) const
Container ToImpl() const
std_msgs::ColorRGBA red
Definition: rviz_colors.h:63
StateAng3d ang
Marker CreateGravityForce(const Vector3d &base_pos) const
Marker CreatePendulum(const Vector3d &base_pos, const EEForces &f_W, const EEPos &pos_W) const
xpp_msgs::TerrainInfo terrain_msg_
Marker CreateSphere(const Vector3d &pos, double diameter=0.03) const
StateLin3d lin
RvizRobotBuilder()
Builds an uninitialized visualizer.
std_msgs::ColorRGBA blue
Definition: rviz_colors.h:63
MarkerVec CreateEEForces(const EEForces &f_W, const EEPos &pos_W, const ContactState &c) const
visualization_msgs::MarkerArray MarkerArray
Marker CreateBox(const Vector3d &pos, Eigen::Quaterniond ori, const Vector3d &edge_length) const
std_msgs::ColorRGBA black
Definition: rviz_colors.h:63
std::vector< Marker > MarkerVec
MarkerVec CreateSupportArea(const ContactState &c, const EEPos &pos_W) const
Marker CreateCopPos(const EEForces &f_W, const EEPos &pos_W) const
Marker CreateForceArrow(const Vector3d &f, const Vector3d &pos) const
const std::string frame_id_
static const int max_ee_
visualization_msgs::Marker Marker
static StateLin3d ToXpp(const xpp_msgs::StateLin3d &ros)


xpp_vis
Author(s): Alexander W. Winkler
autogenerated on Tue Mar 1 2022 00:07:16