Ignition Rendering

API Reference

6.3.1
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 
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 
269  template <class T>
271  const ignition::math::Vector3d &_axis,
272  bool _useParentFrame)
273  {
274  this->axis = _axis;
275  this->useParentFrame = _useParentFrame;
276  this->dirtyAxis = true;
277  }
278 
280  template <class T>
282  {
283  if (this->arrowVisual)
284  {
285  this->arrowVisual->Destroy();
286  this->arrowVisual.reset();
287  }
288 
289  this->arrowVisual = this->Scene()->CreateArrowVisual();
290  this->arrowVisual->SetMaterial("Default/TransYellow");
291  this->arrowVisual->SetLocalPosition(0, 0, 0);
292  this->arrowVisual->SetLocalRotation(0, 0, 0);
293  this->AddChild(this->arrowVisual);
294 
295  this->updateAxis = true;
296  this->ScaleToChild();
297  }
298 
300  template <class T>
302  const ignition::math::Vector3d &_axis,
303  const std::string &_parentName,
304  bool _useParentFrame)
305  {
306  if (this->Type() != JointVisualType::JVT_REVOLUTE2 &&
307  this->Type() != JointVisualType::JVT_UNIVERSAL)
308  {
309  ignlog << "Joint visual is not of type Revolute2 or "
310  << " Universal "
311  << " so the parent axis will not be shown\n";
312  return;
313  }
314 
315  this->parentAxis = _axis;
316  this->parentAxisUseParentFrame = _useParentFrame;
317  this->jointParentName = _parentName;
318  this->dirtyParentAxis = true;
319  }
320 
322  template <class T>
324  {
325  auto jointParentVis = this->Scene()->NodeByName(this->jointParentName);
326  if (jointParentVis == nullptr)
327  {
328  ignlog << "Joint parent with name " << this->jointParentName
329  << " does not exist"
330  << " so the parent axis will not be shown\n";
331  return;
332  }
333 
334  if (this->parentAxisVis)
335  {
336  this->parentAxisVis->Destroy();
337  this->parentAxisVis.reset();
338  }
339 
340  this->parentAxisVis = this->Scene()->CreateJointVisual();
341  jointParentVis->AddChild(this->parentAxisVis);
342  this->parentAxisVis->SetType(this->Type());
343  this->parentAxisVis->SetAxis(this->parentAxis,
344  this->parentAxisUseParentFrame);
345 
346  this->updateParentAxis = true;
347  this->ScaleToChild();
348  }
349 
351  template <class T>
353  bool _useParentFrame)
354  {
355  if (this->ArrowVisual() && this->HasParent())
356  {
357  this->UpdateAxisImpl(this->ArrowVisual(), _axis, _useParentFrame);
358  return true;
359  }
360 
361  return false;
362  }
363 
365  template <class T>
367  const ignition::math::Vector3d &_axis,
368  bool _useParentFrame)
369  {
370  if (this->ParentAxisVisual() &&
371  this->ParentAxisVisual()->ArrowVisual() &&
372  this->ParentAxisVisual()->HasParent())
373  {
374  this->UpdateAxisImpl(this->ParentAxisVisual()->ArrowVisual(),
375  _axis, _useParentFrame);
376  return true;
377  }
378 
379  return false;
380  }
381 
383  template <class T>
385  const ignition::math::Vector3d &_axis,
386  bool _useParentFrame)
387  {
388  // Get rotation to axis vector
389  ignition::math::Vector3d axisDir = _axis;
390  ignition::math::Vector3d u = axisDir.Normalize();
392  double cosTheta = v.Dot(u);
393  double angle = acos(cosTheta);
395  // check the parallel case
396  if (ignition::math::equal(angle, IGN_PI))
397  quat.Axis(u.Perpendicular(), angle);
398  else
399  quat.Axis((v.Cross(u)).Normalize(), angle);
400  _arrowVisual->SetLocalRotation(quat);
401 
402  if (_useParentFrame)
403  {
404  ignition::math::Pose3d parentInitPose =
405  this->Parent()->InitialLocalPose();
406 
407  // get rotation of joint visual in model frame
408  ignition::math::Quaterniond quatFromModel =
409  (parentInitPose * this->LocalPose()).Rot();
410 
411  // rotate arrow visual so that the axis vector applies to the model
412  // frame.
413  _arrowVisual->SetLocalRotation(quatFromModel.Inverse() *
414  _arrowVisual->LocalRotation());
415  }
416 
417  _arrowVisual->ShowArrowRotation(
418  this->Type() == JointVisualType::JVT_REVOLUTE ||
419  this->Type() == JointVisualType::JVT_REVOLUTE2 ||
420  this->Type() == JointVisualType::JVT_UNIVERSAL ||
421  this->Type() == JointVisualType::JVT_GEARBOX);
422 
423  if (this->axisVisual)
424  _arrowVisual->SetVisible(true);
425  else
426  return;
427 
428  // Don't change the visibility of joint child axis
429  if (this->ArrowVisual() != _arrowVisual)
430  return;
431 
432  // Hide existing arrow head if it overlaps with the axis
433  auto axisWorldRotation = _arrowVisual->WorldPose().Rot();
434  auto jointWorldRotation = this->WorldPose().Rot();
435 
436  this->axisVisual->ShowAxisHead(true);
437  _arrowVisual->ShowArrowShaft(true);
438 
439  auto axisWorld = axisWorldRotation * ignition::math::Vector3d::UnitZ;
440  if (axisWorld == jointWorldRotation * ignition::math::Vector3d::UnitX)
441  {
442  this->axisVisual->ShowAxisHead(0, false);
443  _arrowVisual->ShowArrowShaft(false);
444  }
445  else if (axisWorld ==
446  jointWorldRotation * ignition::math::Vector3d::UnitY)
447  {
448  this->axisVisual->ShowAxisHead(1, false);
449  _arrowVisual->ShowArrowShaft(false);
450  }
451  else if (axisWorld ==
452  jointWorldRotation * ignition::math::Vector3d::UnitZ)
453  {
454  this->axisVisual->ShowAxisHead(2, false);
455  _arrowVisual->ShowArrowShaft(false);
456  }
457  }
458 
460  template <class T>
462  {
463  if (!this->HasParent())
464  return;
465 
466  // Joint visual is attached to the child's visual
467  VisualPtr parentVisual =
468  std::dynamic_pointer_cast<Visual>(this->Parent());
469 
470  if (parentVisual)
471  {
472  double childSize =
473  std::max(0.1, parentVisual->BoundingBox().Size().Length());
474  this->scaleToChild = ignition::math::Vector3d(childSize * 0.2,
475  childSize * 0.2, childSize * 0.2);
476  this->SetLocalScale(this->scaleToChild);
477  if (this->ParentAxisVisual())
478  this->ParentAxisVisual()->SetLocalScale(this->scaleToChild);
479  }
480  }
481 
483  template <class T>
485  {
486  this->jointVisualType = _type;
487  this->dirtyJointType = true;
488  }
489 
491  template <class T>
493  {
494  return this->axis;
495  }
496 
498  template <class T>
500  {
501  return this->parentAxis;
502  }
503 
505  template <class T>
507  {
508  return this->jointVisualType;
509  }
510 
512  template <class T>
514  {
515  return this->parentAxisVis;
516  }
517 
519  template <class T>
521  {
522  return this->arrowVisual;
523  }
524 
526  template <class T>
527  void BaseJointVisual<T>::SetVisible(bool _visible)
528  {
529  T::SetVisible(_visible);
530 
531  if (this->ArrowVisual())
532  this->ArrowVisual()->SetVisible(_visible);
533 
534  if (this->Type() == JointVisualType::JVT_REVOLUTE2 ||
535  this->Type() == JointVisualType::JVT_UNIVERSAL)
536  {
537  if (this->ParentAxisVisual())
538  this->ParentAxisVisual()->SetVisible(_visible);
539  }
540 
541  if (this->axisVisual)
542  this->axisVisual->SetVisible(_visible);
543  }
544  }
545  }
546 }
547 #endif
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...
Gearbox joint type.
Definition: JointVisual.hh:60
virtual bool UpdateParentAxis(const ignition::math::Vector3d &_axis, bool _useParentFrame) override
Update the parent axis&#39; arrow visual if it exists.
Definition: BaseJointVisual.hh:366
Represents a arrow composite visual.
Definition: ArrowVisual.hh:31
virtual void SetVisible(bool _visible)=0
Specify if this visual is visible.
virtual ArrowVisualPtr ArrowVisual() const override
Get the arrow visual which represents the axis attached to the child.
Definition: BaseJointVisual.hh:520
#define ignlog
void ScaleToChild()
Scale the joint visual according to the joint&#39;s child.
Definition: BaseJointVisual.hh:461
Revolute2 joint type.
Definition: JointVisual.hh:45
virtual bool UpdateAxis(const ignition::math::Vector3d &_axis, bool _useParentFrame) override
Update an axis&#39; arrow visual.
Definition: BaseJointVisual.hh:352
static const Vector3 UnitZ
Represents a joint visual.
Definition: JointVisual.hh:69
JointVisualType
Enum for JointVisual types.
Definition: JointVisual.hh:36
Base implementation of a joint visual.
Definition: BaseJointVisual.hh:40
virtual ignition::math::Vector3d Axis() const override
Get axis vector.
Definition: BaseJointVisual.hh:492
virtual JointVisualPtr ParentAxisVisual() const override
Get the JointVisual which is attached to the parent.
Definition: BaseJointVisual.hh:513
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
No type.
Definition: JointVisual.hh:39
static const Vector3 UnitY
Vector3 Perpendicular() const
void CreateParentAxis()
Helper function to create parent axis visual.
Definition: BaseJointVisual.hh:323
virtual ~BaseJointVisual()
Destructor.
Definition: BaseJointVisual.hh:180
STL class.
virtual void Destroy() override
Destroy any resources associated with this object. Invoking any other functions after destroying an o...
Definition: BaseJointVisual.hh:243
void Axis(T _ax, T _ay, T _az, T _aa)
T Dot(const Vector3< T > &_v) const
Vector3 Cross(const Vector3< T > &_v) const
Manages a single scene-graph. This class updates scene-wide properties and holds the root scene node...
Definition: Scene.hh:49
virtual void PreRender() override
Prepare this object and any of its children for rendering. This should be called for each object in a...
Definition: BaseJointVisual.hh:186
virtual JointVisualType Type() const override
Get joint visual type.
Definition: BaseJointVisual.hh:506
virtual void SetType(const JointVisualType _type) override
Set type for joint visual.
Definition: BaseJointVisual.hh:484
T dynamic_pointer_cast(T... args)
virtual void SetVisible(bool _visible) override
Specify if this visual is visible.
Definition: BaseJointVisual.hh:527
Universal joint type.
Definition: JointVisual.hh:51
T max(T... args)
void UpdateAxisImpl(ArrowVisualPtr _arrowVisual, const ignition::math::Vector3d &_axis, bool _useParentFrame)
Implementation for updating an axis&#39; arrow visual.
Definition: BaseJointVisual.hh:384
Represents a visual node in a scene graph. A Visual is the only node that can have Geometry and other...
Definition: Visual.hh:33
virtual AxisVisualPtr CreateAxisVisual()=0
Create new axis visual. A unique ID and name will automatically be assigned to the visual...
Vector3< double > Vector3d
static const Vector3 One
virtual void Init() override
Definition: BaseJointVisual.hh:232
static const Vector3 UnitX
virtual ignition::math::Vector3d ParentAxis() const override
Get parent axis vector.
Definition: BaseJointVisual.hh:499
static const Vector3 Zero
virtual JointVisualPtr CreateJointVisual()=0
Create new joint visual. A unique ID and name will automatically be assigned to the Joint visual...
Quaternion< T > Inverse() const
#define IGN_PI
virtual ArrowVisualPtr CreateArrowVisual()=0
Create new arrow visual. A unique ID and name will automatically be assigned to the visual...
Revolute joint type.
Definition: JointVisual.hh:42
void CreateAxis()
Helper function to create axis visual.
Definition: BaseJointVisual.hh:281
BaseJointVisual()
Constructor.
Definition: BaseJointVisual.hh:174
virtual void SetAxis(const ignition::math::Vector3d &_axis, bool _useParentFrame) override
Create an axis and attach it to the joint visual.
Definition: BaseJointVisual.hh:270
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: BaseJointVisual.hh:301