Gazebo Rendering

API Reference

6.6.3
gz/rendering/base/BaseJointVisual.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16  */
17 #ifndef IGNITION_RENDERING_BASE_BASEJOINTVISUAL_HH_
18 #define IGNITION_RENDERING_BASE_BASEJOINTVISUAL_HH_
19 
20 #include <algorithm>
21 #include <string>
22 
23 #include "ignition/common/Console.hh"
24 
31 
32 namespace ignition
33 {
34  namespace rendering
35  {
36  inline namespace IGNITION_RENDERING_VERSION_NAMESPACE {
37  //
39  template <class T>
41  public virtual JointVisual,
42  public virtual T
43  {
45  protected: BaseJointVisual();
46 
48  public: virtual ~BaseJointVisual();
49 
50  // Documentation inherited.
51  protected: virtual void Init() override;
52 
53  // Documentation inherited.
54  protected: virtual void PreRender() override;
55 
56  // Documentation inherited.
57  protected: virtual void Destroy() override;
58 
59  // Documentation inherited.
60  public: virtual void SetAxis(const ignition::math::Vector3d &_axis,
61  bool _useParentFrame) override;
62 
63  // Documentation inherited.
64  public: virtual ignition::math::Vector3d Axis() const override;
65 
66  // Documentation inherited.
67  public: virtual void SetParentAxis(
68  const ignition::math::Vector3d &_axis,
69  const std::string &_parentName,
70  bool _useParentFrame) override;
71 
72  // Documentation inherited.
73  public: virtual ignition::math::Vector3d ParentAxis() const override;
74 
75  // Documentation inherited.
76  public: virtual bool UpdateAxis(const ignition::math::Vector3d &_axis,
77  bool _useParentFrame) override;
78 
79  // Documentation inherited.
80  public: virtual bool UpdateParentAxis(
81  const ignition::math::Vector3d &_axis,
82  bool _useParentFrame) override;
83 
84  // Documentation inherited.
85  public: virtual void SetType(const JointVisualType _type) override;
86 
87  // Documentation inherited.
88  public: virtual JointVisualType Type() const override;
89 
90  // Documentation inherited.
91  public: virtual JointVisualPtr ParentAxisVisual() const override;
92 
93  // Documentation inherited.
94  public: virtual ArrowVisualPtr ArrowVisual() const override;
95 
96  // Documentation inherited.
97  public: virtual void SetVisible(bool _visible) override;
98 
104  protected: void UpdateAxisImpl(ArrowVisualPtr _arrowVisual,
105  const ignition::math::Vector3d &_axis,
106  bool _useParentFrame);
107 
109  protected: void CreateAxis();
110 
112  protected: void CreateParentAxis();
113 
115  protected: void ScaleToChild();
116 
118  protected: JointVisualType jointVisualType =
120 
122  protected: AxisVisualPtr axisVisual = nullptr;
123 
127  protected: ArrowVisualPtr arrowVisual = nullptr;
128 
131  protected: JointVisualPtr parentAxisVis = nullptr;
132 
134  protected: ignition::math::Vector3d scaleToChild =
136 
138  protected: bool dirtyJointType = false;
139 
141  protected: bool dirtyAxis = false;
142 
144  protected: bool dirtyParentAxis = false;
145 
147  protected: ignition::math::Vector3d axis =
149 
152  protected: bool useParentFrame = false;
153 
155  protected: bool updateAxis = false;
156 
158  protected: ignition::math::Vector3d parentAxis =
160 
162  protected: std::string jointParentName = "";
163 
166  protected: bool parentAxisUseParentFrame = false;
167 
169  protected: bool updateParentAxis = false;
170  };
171 
173  template <class T>
175  {
176  }
177 
179  template <class T>
181  {
182  }
183 
185  template <class T>
187  {
188  T::PreRender();
189 
190  if (this->ParentAxisVisual())
191  {
192  this->ParentAxisVisual()->PreRender();
193  }
194 
195  if (this->dirtyJointType)
196  {
197  this->UpdateAxis(this->axis, this->useParentFrame);
198  this->UpdateParentAxis(this->parentAxis,
199  this->parentAxisUseParentFrame);
200 
201  this->dirtyJointType = false;
202  }
203 
204  if (this->dirtyAxis)
205  {
206  this->CreateAxis();
207  this->dirtyAxis = false;
208  }
209 
210  if (this->dirtyParentAxis)
211  {
212  this->CreateParentAxis();
213  this->dirtyParentAxis = false;
214  }
215 
216  if (this->updateAxis)
217  {
218  this->updateAxis =
219  !this->UpdateAxis(this->axis, this->useParentFrame);
220  }
221 
222  if (this->updateParentAxis)
223  {
224  this->updateParentAxis =
225  !this->UpdateParentAxis(this->parentAxis,
226  this->parentAxisUseParentFrame);
227  }
228  }
229 
231  template <class T>
233  {
234  T::Init();
235 
236  this->axisVisual = this->Scene()->CreateAxisVisual();
237  this->AddChild(this->axisVisual);
238  this->SetInheritScale(false);
239  }
240 
242  template <class T>
244  {
245  if (this->arrowVisual != nullptr)
246  {
247  this->arrowVisual->Destroy();
248  this->arrowVisual.reset();
249  }
250 
251  if (this->axisVisual != nullptr)
252  {
253  this->axisVisual->Destroy();
254  this->axisVisual.reset();
255  }
256 
257  if (this->parentAxisVis != nullptr)
258  {
259  this->parentAxisVis->Destroy();
260  this->parentAxisVis.reset();
261  }
262 
263  this->dirtyJointType = false;
264  this->dirtyAxis = false;
265  this->dirtyParentAxis = false;
266 
267  T::Destroy();
268  }
269 
271  template <class T>
273  const ignition::math::Vector3d &_axis,
274  bool _useParentFrame)
275  {
276  this->axis = _axis;
277  this->useParentFrame = _useParentFrame;
278  this->dirtyAxis = true;
279  }
280 
282  template <class T>
284  {
285  if (this->arrowVisual)
286  {
287  this->arrowVisual->Destroy();
288  this->arrowVisual.reset();
289  }
290 
291  this->arrowVisual = this->Scene()->CreateArrowVisual();
292  this->arrowVisual->SetMaterial("Default/TransYellow");
293  this->arrowVisual->SetLocalPosition(0, 0, 0);
294  this->arrowVisual->SetLocalRotation(0, 0, 0);
295  this->AddChild(this->arrowVisual);
296 
297  this->updateAxis = true;
298  this->ScaleToChild();
299  }
300 
302  template <class T>
304  const ignition::math::Vector3d &_axis,
305  const std::string &_parentName,
306  bool _useParentFrame)
307  {
308  if (this->Type() != JointVisualType::JVT_REVOLUTE2 &&
309  this->Type() != JointVisualType::JVT_UNIVERSAL)
310  {
311  ignlog << "Joint visual is not of type Revolute2 or "
312  << " Universal "
313  << " so the parent axis will not be shown\n";
314  return;
315  }
316 
317  this->parentAxis = _axis;
318  this->parentAxisUseParentFrame = _useParentFrame;
319  this->jointParentName = _parentName;
320  this->dirtyParentAxis = true;
321  }
322 
324  template <class T>
326  {
327  auto jointParentVis = this->Scene()->NodeByName(this->jointParentName);
328  if (jointParentVis == nullptr)
329  {
330  ignlog << "Joint parent with name " << this->jointParentName
331  << " does not exist"
332  << " so the parent axis will not be shown\n";
333  return;
334  }
335 
336  if (this->parentAxisVis)
337  {
338  this->parentAxisVis->Destroy();
339  this->parentAxisVis.reset();
340  }
341 
342  this->parentAxisVis = this->Scene()->CreateJointVisual();
343  jointParentVis->AddChild(this->parentAxisVis);
344  this->parentAxisVis->SetType(this->Type());
345  this->parentAxisVis->SetAxis(this->parentAxis,
346  this->parentAxisUseParentFrame);
347 
348  this->updateParentAxis = true;
349  this->ScaleToChild();
350  }
351 
353  template <class T>
355  bool _useParentFrame)
356  {
357  if (this->ArrowVisual() && this->HasParent())
358  {
359  this->UpdateAxisImpl(this->ArrowVisual(), _axis, _useParentFrame);
360  return true;
361  }
362 
363  return false;
364  }
365 
367  template <class T>
369  const ignition::math::Vector3d &_axis,
370  bool _useParentFrame)
371  {
372  if (this->ParentAxisVisual() &&
373  this->ParentAxisVisual()->ArrowVisual() &&
374  this->ParentAxisVisual()->HasParent())
375  {
376  this->UpdateAxisImpl(this->ParentAxisVisual()->ArrowVisual(),
377  _axis, _useParentFrame);
378  return true;
379  }
380 
381  return false;
382  }
383 
385  template <class T>
387  const ignition::math::Vector3d &_axis,
388  bool _useParentFrame)
389  {
390  // Get rotation to axis vector
391  ignition::math::Vector3d axisDir = _axis;
392  ignition::math::Vector3d u = axisDir.Normalize();
394  double cosTheta = v.Dot(u);
395  double angle = acos(cosTheta);
397  // check the parallel case
398  if (ignition::math::equal(angle, IGN_PI))
399  quat.Axis(u.Perpendicular(), angle);
400  else
401  quat.Axis((v.Cross(u)).Normalize(), angle);
402  _arrowVisual->SetLocalRotation(quat);
403 
404  if (_useParentFrame)
405  {
406  ignition::math::Pose3d parentInitPose =
407  this->Parent()->InitialLocalPose();
408 
409  // get rotation of joint visual in model frame
410  ignition::math::Quaterniond quatFromModel =
411  (parentInitPose * this->LocalPose()).Rot();
412 
413  // rotate arrow visual so that the axis vector applies to the model
414  // frame.
415  _arrowVisual->SetLocalRotation(quatFromModel.Inverse() *
416  _arrowVisual->LocalRotation());
417  }
418 
419  _arrowVisual->ShowArrowRotation(
420  this->Type() == JointVisualType::JVT_REVOLUTE ||
421  this->Type() == JointVisualType::JVT_REVOLUTE2 ||
422  this->Type() == JointVisualType::JVT_UNIVERSAL ||
423  this->Type() == JointVisualType::JVT_GEARBOX);
424 
425  if (this->axisVisual)
426  _arrowVisual->SetVisible(true);
427  else
428  return;
429 
430  // Don't change the visibility of joint child axis
431  if (this->ArrowVisual() != _arrowVisual)
432  return;
433 
434  // Hide existing arrow head if it overlaps with the axis
435  auto axisWorldRotation = _arrowVisual->WorldPose().Rot();
436  auto jointWorldRotation = this->WorldPose().Rot();
437 
438  this->axisVisual->ShowAxisHead(true);
439  _arrowVisual->ShowArrowShaft(true);
440 
441  auto axisWorld = axisWorldRotation * ignition::math::Vector3d::UnitZ;
442  if (axisWorld == jointWorldRotation * ignition::math::Vector3d::UnitX)
443  {
444  this->axisVisual->ShowAxisHead(0, false);
445  _arrowVisual->ShowArrowShaft(false);
446  }
447  else if (axisWorld ==
448  jointWorldRotation * ignition::math::Vector3d::UnitY)
449  {
450  this->axisVisual->ShowAxisHead(1, false);
451  _arrowVisual->ShowArrowShaft(false);
452  }
453  else if (axisWorld ==
454  jointWorldRotation * ignition::math::Vector3d::UnitZ)
455  {
456  this->axisVisual->ShowAxisHead(2, false);
457  _arrowVisual->ShowArrowShaft(false);
458  }
459  }
460 
462  template <class T>
464  {
465  if (!this->HasParent())
466  return;
467 
468  // Joint visual is attached to the child's visual
469  VisualPtr parentVisual =
470  std::dynamic_pointer_cast<Visual>(this->Parent());
471 
472  if (parentVisual)
473  {
474  double childSize =
475  std::max(0.1, parentVisual->BoundingBox().Size().Length());
476  this->scaleToChild = ignition::math::Vector3d(childSize * 0.2,
477  childSize * 0.2, childSize * 0.2);
478  this->SetLocalScale(this->scaleToChild);
479  if (this->ParentAxisVisual())
480  this->ParentAxisVisual()->SetLocalScale(this->scaleToChild);
481  }
482  }
483 
485  template <class T>
487  {
488  this->jointVisualType = _type;
489  this->dirtyJointType = true;
490  }
491 
493  template <class T>
495  {
496  return this->axis;
497  }
498 
500  template <class T>
502  {
503  return this->parentAxis;
504  }
505 
507  template <class T>
509  {
510  return this->jointVisualType;
511  }
512 
514  template <class T>
516  {
517  return this->parentAxisVis;
518  }
519 
521  template <class T>
523  {
524  return this->arrowVisual;
525  }
526 
528  template <class T>
529  void BaseJointVisual<T>::SetVisible(bool _visible)
530  {
531  T::SetVisible(_visible);
532 
533  if (this->ArrowVisual())
534  this->ArrowVisual()->SetVisible(_visible);
535 
536  if (this->Type() == JointVisualType::JVT_REVOLUTE2 ||
537  this->Type() == JointVisualType::JVT_UNIVERSAL)
538  {
539  if (this->ParentAxisVisual())
540  this->ParentAxisVisual()->SetVisible(_visible);
541  }
542 
543  if (this->axisVisual)
544  this->axisVisual->SetVisible(_visible);
545  }
546  }
547  }
548 }
549 #endif
virtual ~BaseJointVisual()
Destructor.
Definition: gz/rendering/base/BaseJointVisual.hh:180
virtual JointVisualPtr CreateJointVisual()=0
Create new joint visual. A unique ID and name will automatically be assigned to the Joint visual.
virtual bool UpdateAxis(const ignition::math::Vector3d &_axis, bool _useParentFrame) override
Update an axis' arrow visual.
Definition: gz/rendering/base/BaseJointVisual.hh:354
virtual JointVisualPtr ParentAxisVisual() const override
Get the JointVisual which is attached to the parent.
Definition: gz/rendering/base/BaseJointVisual.hh:515
Base implementation of a joint visual.
Definition: gz/rendering/base/BaseJointVisual.hh:40
STL class.
STL class.
Vector3 Perpendicular() const
#define IGN_PI
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
virtual void SetType(const JointVisualType _type) override
Set type for joint visual.
Definition: gz/rendering/base/BaseJointVisual.hh:486
void CreateAxis()
Helper function to create axis visual.
Definition: gz/rendering/base/BaseJointVisual.hh:283
virtual void SetAxis(const ignition::math::Vector3d &_axis, bool _useParentFrame) override
Create an axis and attach it to the joint visual.
Definition: gz/rendering/base/BaseJointVisual.hh:272
void Axis(const Vector3< T > &_axis, T _a)
virtual void Init() override
Definition: gz/rendering/base/BaseJointVisual.hh:232
#define ignlog
static const Vector3 One
bool updateAxis
Flag to update the axis visual.
Definition: gz/rendering/base/BaseJointVisual.hh:155
Represents a joint visual.
Definition: gz/rendering/JointVisual.hh:69
bool parentAxisUseParentFrame
Flag to indicate whether parent axis vector is expressed in joint parent frame.
Definition: gz/rendering/base/BaseJointVisual.hh:166
virtual ArrowVisualPtr ArrowVisual() const override
Get the arrow visual which represents the axis attached to the child.
Definition: gz/rendering/base/BaseJointVisual.hh:522
Manages a single scene-graph. This class updates scene-wide properties and holds the root scene node....
Definition: gz/rendering/Scene.hh:49
@ JVT_GEARBOX
Gearbox joint type.
Definition: gz/rendering/JointVisual.hh:60
static const Vector3 Zero
virtual void SetVisible(bool _visible) override
Specify if this visual is visible.
Definition: gz/rendering/base/BaseJointVisual.hh:529
JointVisualType
Enum for JointVisual types.
Definition: gz/rendering/JointVisual.hh:36
static const Vector3 UnitY
@ JVT_REVOLUTE2
Revolute2 joint type.
Definition: gz/rendering/JointVisual.hh:45
bool useParentFrame
Flag to indicate whether axis vector is expressed in joint parent frame.
Definition: gz/rendering/base/BaseJointVisual.hh:152
ignition::math::Vector3d parentAxis
Parent axis vector.
Definition: gz/rendering/base/BaseJointVisual.hh:158
ArrowVisualPtr arrowVisual
The visual representing the one joint axis. There can be only one axis visual per joint visual,...
Definition: gz/rendering/base/BaseJointVisual.hh:127
virtual bool UpdateParentAxis(const ignition::math::Vector3d &_axis, bool _useParentFrame) override
Update the parent axis' arrow visual if it exists.
Definition: gz/rendering/base/BaseJointVisual.hh:368
@ JVT_UNIVERSAL
Universal joint type.
Definition: gz/rendering/JointVisual.hh:51
bool dirtyAxis
Flag to indicate axis data has changed.
Definition: gz/rendering/base/BaseJointVisual.hh:141
ignition::math::Vector3d axis
Joint visual axis vector.
Definition: gz/rendering/base/BaseJointVisual.hh:147
BaseJointVisual()
Constructor.
Definition: gz/rendering/base/BaseJointVisual.hh:174
virtual ignition::math::Vector3d ParentAxis() const override
Get parent axis vector.
Definition: gz/rendering/base/BaseJointVisual.hh:501
AxisVisualPtr axisVisual
The joint's XYZ frame visual.
Definition: gz/rendering/base/BaseJointVisual.hh:122
virtual void PreRender() override
Prepare this object and any of its children for rendering. This should be called for each object in a...
Definition: gz/rendering/base/BaseJointVisual.hh:186
static const Vector3 UnitX
virtual void SetVisible(bool _visible)=0
Specify if this visual is visible.
static const Vector3 UnitZ
std::string jointParentName
Joint parent name.
Definition: gz/rendering/base/BaseJointVisual.hh:162
virtual void Destroy() override
Destroy any resources associated with this object. Invoking any other functions after destroying an o...
Definition: gz/rendering/base/BaseJointVisual.hh:243
Quaternion< T > Inverse() const
virtual ArrowVisualPtr CreateArrowVisual()=0
Create new arrow visual. A unique ID and name will automatically be assigned to the visual.
T Dot(const Vector3< T > &_v) const
JointVisualType jointVisualType
Type of joint visualization.
Definition: gz/rendering/base/BaseJointVisual.hh:118
@ JVT_REVOLUTE
Revolute joint type.
Definition: gz/rendering/JointVisual.hh:42
virtual void SetParentAxis(const ignition::math::Vector3d &_axis, const std::string &_parentName, bool _useParentFrame) override
Create a parent axis for hinge2 and universal joint types and attach it to the joint visual.
Definition: gz/rendering/base/BaseJointVisual.hh:303
ignition::math::Vector3d scaleToChild
Scale based on the size of the joint's child.
Definition: gz/rendering/base/BaseJointVisual.hh:134
void CreateParentAxis()
Helper function to create parent axis visual.
Definition: gz/rendering/base/BaseJointVisual.hh:325
JointVisualPtr parentAxisVis
Second joint visual for hinge2 and universal joints. It is a simplified visual without an XYZ frame.
Definition: gz/rendering/base/BaseJointVisual.hh:131
bool dirtyJointType
Flag to indicate joint visual type has changed.
Definition: gz/rendering/base/BaseJointVisual.hh:138
bool updateParentAxis
Flag to update the parent axis visual.
Definition: gz/rendering/base/BaseJointVisual.hh:169
T max(T... args)
void UpdateAxisImpl(ArrowVisualPtr _arrowVisual, const ignition::math::Vector3d &_axis, bool _useParentFrame)
Implementation for updating an axis' arrow visual.
Definition: gz/rendering/base/BaseJointVisual.hh:386
virtual JointVisualType Type() const override
Get joint visual type.
Definition: gz/rendering/base/BaseJointVisual.hh:508
void ScaleToChild()
Scale the joint visual according to the joint's child.
Definition: gz/rendering/base/BaseJointVisual.hh:463
virtual ignition::math::Vector3d Axis() const override
Get axis vector.
Definition: gz/rendering/base/BaseJointVisual.hh:494
bool dirtyParentAxis
Flag to indicate parent axis data has changed.
Definition: gz/rendering/base/BaseJointVisual.hh:144
Vector3< double > Vector3d
@ JVT_NONE
No type.
Definition: gz/rendering/JointVisual.hh:39
virtual NodePtr NodeByName(const std::string &_name) const =0
Get node with the given name. If no node exists with the given name, NULL will be returned.
virtual AxisVisualPtr CreateAxisVisual()=0
Create new axis visual. A unique ID and name will automatically be assigned to the visual.
Vector3 Cross(const Vector3< T > &_v) const
Represents a arrow composite visual.
Definition: gz/rendering/ArrowVisual.hh:31