New trajectory follower system #1332

caguero · 2022-02-10T16:50:06Z

Signed-off-by: Carlos Agüero [email protected]

🎉 New feature

Summary

This system adds a very basic trajectory follower applied to a model. The trajectory is performed by applying forces and torques to a link. It's possible to specify the trajectory in three ways:

Providing a set of waypoints.
Specifying a segment relative to the starting pose of the model.
Specifying a circle relative to the starting pose of the model.

Note that trajectories are in 2D.

Test it

Run the trajectory_follower.sdf example to see it in action with a box.

You can see a more interesting example here.

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
codecheck passed (See contributing)
All tests passed (See test coverage)
While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

Signed-off-by: Carlos Agüero <[email protected]>

iche033

works well for me after fixing the force that's applied to objects.
left a couple of other minor suggestions.

iche033 · 2022-02-10T17:59:43Z

examples/worlds/trajectory_follower.sdf

+        filename="ignition-gazebo-trajectory-follower-system"
+        name="ignition::gazebo::systems::TrajectoryFollower">
+        <link_name>box_link</link_name>
+        <loop_forever>true</loop_forever>


suggestion to name this <loop> for consistency with actor's loop param

Changed in f082490.

iche033 · 2022-02-10T18:01:56Z

src/systems/trajectory_follower/TrajectoryFollower.cc

+  if (this->dataPtr->localWaypoints.empty())
+    return;
+
+  //this->dataPtr->modelPose = this->dataPtr->link.WorldPose(_ecm);


Removed in f082490.

iche033 · 2022-02-10T18:02:04Z

src/systems/trajectory_follower/TrajectoryFollower.cc

+  //this->dataPtr->modelPose = this->dataPtr->link.WorldPose(_ecm);
+  this->dataPtr->modelPose = ignition::gazebo::worldPose(
+    this->dataPtr->link.Entity(), _ecm);
+  // (*this->dataPtr->modelPose).Pos().Z() = 0;


Removed in f082490.

iche033 · 2022-02-10T18:12:33Z

src/systems/trajectory_follower/TrajectoryFollower.cc

+
+  // Parse the optional <torque> element.
+  if (_sdf->HasElement("torque"))
+    this->forceToApply = _sdf->Get<double>("torque");


forceToApply -> torqueToApply

after fixing this, I made both <force> and <torque> in demo world file to be 10 and the box moves nicely without too much overshooting when rotating

Good catch, changed in f082490.

iche033 · 2022-02-10T18:33:20Z

src/systems/trajectory_follower/TrajectoryFollower.hh

+  ///         The block should contain the relative direction and distance from
+  ///         the initial position in which the vehicle should move, specified
+  ///         in the world frame.
+  ///           <direction>: Relative direction (degrees) in the world frame for


I think most of our angles are specified in radians? For the <pose> element in particular, there is a bool degrees attribute to indicate if it's degrees or not. Don't have strong opinion on this though.

Changed to radians in f082490.

thanks, I updated doc in 0927042

Signed-off-by: Carlos Agüero <[email protected]>

Signed-off-by: Ian Chen <[email protected]>

iche033

looks good to me.

iche033 · 2022-02-11T23:50:35Z

there's a windows build error:

gazebo\src\systems\trajectory_follower\TrajectoryFollower.cc(168,20): error C2065: 'M_PI': undeclared identifier

Signed-off-by: Ian Chen <[email protected]>

iche033 · 2022-02-12T00:37:30Z

pushed a fix in 9a1ddd9

Signed-off-by: Ian Chen <[email protected]>

codecov · 2022-02-12T06:19:57Z

Codecov Report

Merging #1332 (8d365d0) into ign-gazebo6 (77dd855) will not change coverage.
The diff coverage is n/a.

@@             Coverage Diff              @@
##           ign-gazebo6    #1332   +/-   ##
============================================
  Coverage        62.06%   62.06%           
============================================
  Files              278      278           
  Lines            23378    23378           
============================================
  Hits             14510    14510           
  Misses            8868     8868

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 77dd855...8d365d0. Read the comment docs.

osrf-triage · 2022-03-01T18:31:08Z

This pull request has been mentioned on Gazebo Community. There might be relevant details there:

https://community.gazebosim.org/t/new-ignition-releases-2022-03-01-citadel-edifice-fortress/1313/1

chapulina · 2022-03-09T21:11:49Z

I'm porting this forward in #1380 and it doesn't work out of the box in Garden. I'm looking into it.

I also noticed that the plugin was added without a test. We should add one.

New trajectory follower system

7d4d4a5

Signed-off-by: Carlos Agüero <[email protected]>

caguero requested a review from chapulina as a code owner February 10, 2022 16:50

github-actions bot added the 🏯 fortress Ignition Fortress label Feb 10, 2022

iche033 reviewed Feb 10, 2022

View reviewed changes

caguero added 2 commits February 11, 2022 17:37

Tweaks

f082490

Signed-off-by: Carlos Agüero <[email protected]>

Tweaks

b23b5b7

Signed-off-by: Carlos Agüero <[email protected]>

caguero mentioned this pull request Feb 11, 2022

Autonomy for Vessel A osrf/mbzirc#42

Merged

update doc on line direction

0927042

Signed-off-by: Ian Chen <[email protected]>

iche033 approved these changes Feb 11, 2022

View reviewed changes

use IGN_PI

9a1ddd9

Signed-off-by: Ian Chen <[email protected]>

fix codecheck errors

8d365d0

Signed-off-by: Ian Chen <[email protected]>

caguero merged commit 49d0673 into ign-gazebo6 Feb 13, 2022

caguero deleted the caguero/trajectory_follower branch February 13, 2022 18:03

iche033 restored the caguero/trajectory_follower branch February 17, 2022 20:53

iche033 deleted the caguero/trajectory_follower branch February 17, 2022 20:55

chapulina mentioned this pull request Mar 10, 2022

Fix trajectory follower: enable WorldPose component #1382

Merged

7 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

New trajectory follower system #1332

New trajectory follower system #1332

caguero commented Feb 10, 2022

iche033 left a comment

iche033 Feb 10, 2022

caguero Feb 11, 2022

iche033 Feb 10, 2022

caguero Feb 11, 2022

iche033 Feb 10, 2022

caguero Feb 11, 2022

iche033 Feb 10, 2022

iche033 Feb 10, 2022

caguero Feb 11, 2022

iche033 Feb 10, 2022

caguero Feb 11, 2022

iche033 Feb 11, 2022

iche033 left a comment

iche033 commented Feb 11, 2022

iche033 commented Feb 12, 2022

codecov bot commented Feb 12, 2022 •

edited

Loading

osrf-triage commented Mar 1, 2022

chapulina commented Mar 9, 2022

New trajectory follower system #1332

New trajectory follower system #1332

Conversation

caguero commented Feb 10, 2022

🎉 New feature

Summary

Test it

Checklist

iche033 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

iche033 left a comment

Choose a reason for hiding this comment

iche033 commented Feb 11, 2022

iche033 commented Feb 12, 2022

codecov bot commented Feb 12, 2022 • edited Loading

Codecov Report

osrf-triage commented Mar 1, 2022

chapulina commented Mar 9, 2022

codecov bot commented Feb 12, 2022 •

edited

Loading