Set link velocity from `set_model_state` plugin #2440

yaswanth1701 · 2024-06-08T21:14:06Z

🎉 New feature

Follow up PR to #2359

Summary

This PR introduces WorldLinearVelocityReset and WorldAngularVelocityReset components for the LInk entity to set initial link velocities from SDF using set_model_state plugin. The link velocities are only set to root link of the FreeGroup for now.

Test it

gz sim -v 4 examples/worlds/set_model_state.sdf

Should see the red cube moving and rotating.

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
Consider updating Python bindings (if the library has them)
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: yaswanth1701 <[email protected]>

…onents Signed-off-by: yaswanth1701 <[email protected]>

Signed-off-by: yaswanth1701 <[email protected]>

ahcorde · 2024-06-10T07:56:10Z

include/gz/sim/components/WorldAngularVelocityReset.hh

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2019 Open Source Robotics Foundation


Suggested change

* Copyright (C) 2019 Open Source Robotics Foundation

* Copyright (C) 2024 Open Source Robotics Foundation

ahcorde · 2024-06-10T07:57:00Z

include/gz/sim/components/WorldAngularVelocityReset.hh

+ */
+#ifndef GZ_SIM_COMPONENTS_WORLDANGULARVELOCITYRESET_HH_
+#define GZ_SIM_COMPONENTS_WORLDANGULARELOCITYRESET_HH_
+#include <vector>


remove, this is not used in this header

ahcorde · 2024-06-10T07:57:16Z

include/gz/sim/components/WorldLinearVelocityReset.hh

@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2019 Open Source Robotics Foundation


Suggested change

* Copyright (C) 2019 Open Source Robotics Foundation

* Copyright (C) 2024 Open Source Robotics Foundation

ahcorde · 2024-06-10T07:57:25Z

include/gz/sim/components/WorldLinearVelocityReset.hh

+ */
+#ifndef GZ_SIM_COMPONENTS_WORLDLINEARVELOCITYRESET_HH_
+#define GZ_SIM_COMPONENTS_WORLDLINEARVELOCITYRESET_HH_
+#include <vector>


remove, this is not used in this header

ahcorde · 2024-06-10T07:57:41Z

src/systems/physics/Physics.cc

@@ -2591,7 +2593,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
        return true;
      });

-  // Update link linear velocity
+    // Update link linear velocity


Suggested change

// Update link linear velocity

// Update link linear velocity

ahcorde · 2024-06-10T07:59:31Z

src/systems/physics/Physics.cc

+          return true;
+
+        auto rootLinkPtr = freeGroup->RootLink();
+        if(rootLinkPtr != linkPtrPhys){


Suggested change

if(rootLinkPtr != linkPtrPhys){

if(rootLinkPtr != linkPtrPhys)

{

ahcorde · 2024-06-10T07:59:48Z

src/systems/physics/Physics.cc

+          gzwarn << "Attempting to set angular velocity for link ["<< _entity
+          <<"] which is not root link of the FreeGroup."
+          <<"Velocity won't be set."<< std::endl;


Suggested change

gzwarn << "Attempting to set angular velocity for link ["<< _entity

<<"] which is not root link of the FreeGroup."

<<"Velocity won't be set."<< std::endl;

gzwarn << "Attempting to set angular velocity for link [ " << _entity

<< " ] which is not root link of the FreeGroup."

<< "Velocity won't be set." << std::endl;

ahcorde · 2024-06-10T07:59:58Z

src/systems/physics/Physics.cc

+            gzdbg << "Attempting to set link angular velocity, but the "
+                   << "physics engine doesn't support velocity commands. "
+                   << "Velocity won't be set."
+                   << std::endl;


Suggested change

gzdbg << "Attempting to set link angular velocity, but the "

<< "physics engine doesn't support velocity commands. "

<< "Velocity won't be set."

<< std::endl;

gzdbg << "Attempting to set link angular velocity, but the "

<< "physics engine doesn't support velocity commands. "

<< "Velocity won't be set."

<< std::endl;

ahcorde · 2024-06-10T08:00:06Z

src/systems/set_model_state/SetModelState.cc

+
+

ahcorde · 2024-06-10T08:01:01Z

src/systems/set_model_state/SetModelState.cc

+      }
+    }
+
+       }


wierd indentation

Signed-off-by: yaswanth1701 <[email protected]>

yaswanth1701 · 2024-06-11T06:32:49Z

Hi Alejandro, I have made the required changes, can you please review them again.

Signed-off-by: yaswanth1701 <[email protected]>

scpeters · 2024-06-20T15:52:07Z

src/systems/physics/Physics.cc

+          worldAngularVelFeature->SetWorldAngularVelocity(
+              math::eigen3::convert(worldAngularVel));
+          this->angularVelocityResetFlag = true;
+        }


I see that this is your logic for ensuring that the VelocityReset components are only acted on a single time, but I recommend following the method used for the JointVelocityReset component, which is to delete the component after it has been acted on:

gz-sim/src/systems/physics/Physics.cc

Lines 3703 to 3714 in 0066023

std::vector<Entity> entitiesVelocityReset;

_ecm.Each<components::JointVelocityReset>(

[&](const Entity &_entity, components::JointVelocityReset *) -> bool

{

entitiesVelocityReset.push_back(_entity);

return true;

});

for (const auto entity : entitiesVelocityReset)

{

_ecm.RemoveComponent<components::JointVelocityReset>(entity);

}

scpeters

for consistency with the other component header files, we could name them simply AngularVelocityReset.hh and LinearVelocityReset.hh. The other header files also define components without the World prefix that refer to velocity in a body-fixed frame. We can leave a TODO comment for now if we don't want to implement that yet

Signed-off-by: yaswanth1701 <[email protected]>

scpeters · 2024-06-27T23:52:38Z

examples/worlds/set_model_state.sdf

+            <iyz>0</iyz>
+            <izz>5</izz>
+          </inertia>
+          <mass>120.0</mass>


I updated #2359 to use auto-inertials in the example world in 7cab1bb

I think we should use a box with 3 different dimensions rather than a cube in order to visualize the Dzhanibekov effect. I think the dimensions from the boxes benchmark test (0.1, 0.4, 0.9) and initial angular velocity with largest component in Y (0.1, 5.0, 0.1) would work well. With the updated dimensions, we could use auto-inertials as well.

scpeters · 2024-06-27T23:54:47Z

examples/worlds/set_model_state.sdf

+        <model_state>
+         <link_state name="box_link">
+           <linear_velocity>0.1 -0.1 0.1</linear_velocity>
+           <angular_velocity>1 2 1</angular_velocity>


I recommend adding a degrees boolean attribute to //link_state/angular_velocity to match the behavior of the //joint_state/position/@degrees and //joint_state/velocity/@degrees attributes. Then it would be nice to have pair of example boxes with angular velocity expressed in both deg/s and rad/s

got it, I will make the changes

Signed-off-by: yaswanth1701 <[email protected]>

scpeters · 2024-07-19T23:38:53Z

@yaswanth1701 can you open a separate pull request targeting the main branch with just the new components and the changes to the Physics system? Feature freeze is next Friday, which means pull requests with new features for Ionic need to be opened by that date. A test for the changes to the Physics system can be added after the pull request is opened.

yaswanth1701 · 2024-07-20T06:40:43Z

@yaswanth1701 can you open a separate pull request targeting the main branch with just the new components and the changes to the Physics system? Feature freeze is next Friday, which means pull requests with new features for Ionic need to be opened by that date. A test for the changes to the Physics system can be added after the pull request is opened.

Yeah sure, I will try to do this by tomorrow.

Signed-off-by: yaswanth1701 <[email protected]>

…pe' into scpeters/set_model_state_prototype

scpeters · 2024-08-15T08:20:21Z

I just resolved conflicts since #2489 was merged

scpeters

merging into my branch

yaswanth1701 added 5 commits June 6, 2024 19:35

modified velocity reset components

b062cc1

Signed-off-by: yaswanth1701 <[email protected]>

updated physic plugin to set initial link velocities

bb213b5

Signed-off-by: yaswanth1701 <[email protected]>

updated set_model_state plugin for creating link velocity resert comp…

53e2b33

…onents Signed-off-by: yaswanth1701 <[email protected]>

updated model state sdf to test link velocity reset component

d46c143

Signed-off-by: yaswanth1701 <[email protected]>

added root link check for link

5b73d20

Signed-off-by: yaswanth1701 <[email protected]>

yaswanth1701 requested review from azeey and mjcarroll as code owners June 8, 2024 21:14

yaswanth1701 added 7 commits June 9, 2024 02:48

pre commit hooks

72d75f9

Signed-off-by: yaswanth1701 <[email protected]>

fixed line length

7baa3b8

Signed-off-by: yaswanth1701 <[email protected]>

minor changes

116a728

Signed-off-by: yaswanth1701 <[email protected]>

added inertia to box in set_model_state sdf

327eaa3

Signed-off-by: yaswanth1701 <[email protected]>

minor changes

9f3555d

Signed-off-by: yaswanth1701 <[email protected]>

minor changes

1a390c0

Signed-off-by: yaswanth1701 <[email protected]>

minor changes

8e730cc

Signed-off-by: yaswanth1701 <[email protected]>

ahcorde requested changes Jun 10, 2024

View reviewed changes

yaswanth1701 added 2 commits June 11, 2024 11:49

resolved formatting comments

3db0e98

Signed-off-by: yaswanth1701 <[email protected]>

resolved linting error

b9e559b

Signed-off-by: yaswanth1701 <[email protected]>

yaswanth1701 added 3 commits June 17, 2024 18:42

added flag to account for reset components only once

b8deab3

Signed-off-by: yaswanth1701 <[email protected]>

added zero gravity to sdf

778b2fb

Signed-off-by: yaswanth1701 <[email protected]>

fixed linting errors

0066023

Signed-off-by: yaswanth1701 <[email protected]>

scpeters reviewed Jun 20, 2024

View reviewed changes

yaswanth1701 added 2 commits June 22, 2024 23:00

removed reset components in updatesim physics.cc

37b026c

Signed-off-by: yaswanth1701 <[email protected]>

fixes linting errors

4cebd44

Signed-off-by: yaswanth1701 <[email protected]>

scpeters reviewed Jun 27, 2024

View reviewed changes

yaswanth1701 added 4 commits July 1, 2024 00:30

added degree option for angular velocity in set_model_state system

44554a2

Signed-off-by: yaswanth1701 <[email protected]>

resolved linting errors

b335da9

Signed-off-by: yaswanth1701 <[email protected]>

added some comments to sdf files

51f04cc

Signed-off-by: yaswanth1701 <[email protected]>

changes to degree to radian conversion

3dd6404

Signed-off-by: yaswanth1701 <[email protected]>

yaswanth1701 added 2 commits July 11, 2024 20:48

fixed linting errors

cdee544

Signed-off-by: yaswanth1701 <[email protected]>

fixed linting errors

c9d6690

Signed-off-by: yaswanth1701 <[email protected]>

scpeters mentioned this pull request Jul 20, 2024

Spec 1.12: link_state, joint_state changes gazebosim/sdformat#1461

Merged

9 tasks

yaswanth1701 mentioned this pull request Jul 22, 2024

Physics: set link velocity from *VelocityReset components #2489

Merged

9 tasks

azeey added the beta Targeting beta release of upcoming collection label Jul 29, 2024

yaswanth1701 and others added 2 commits August 8, 2024 13:48

minor changes

6cf3856

Signed-off-by: yaswanth1701 <[email protected]>

Merge remote-tracking branch 'origin/scpeters/set_model_state_prototy…

9de189c

…pe' into scpeters/set_model_state_prototype

scpeters approved these changes Aug 15, 2024

View reviewed changes

scpeters changed the title ~~Added world linear and angular velocity reset components for link to set_model_state plugin~~ Set link velocity from set_model_state plugin Aug 15, 2024

scpeters merged commit 0adf71d into gazebosim:scpeters/set_model_state_prototype Aug 15, 2024
4 of 6 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Set link velocity from `set_model_state` plugin #2440

Set link velocity from `set_model_state` plugin #2440

yaswanth1701 commented Jun 8, 2024 •

edited

Loading

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

ahcorde Jun 10, 2024

yaswanth1701 commented Jun 11, 2024

scpeters Jun 20, 2024

scpeters left a comment

scpeters Jun 27, 2024

scpeters Jun 27, 2024

yaswanth1701 Jun 29, 2024 •

edited

Loading

scpeters commented Jul 19, 2024

yaswanth1701 commented Jul 20, 2024

scpeters commented Aug 15, 2024

scpeters left a comment

		@@ -0,0 +1,48 @@
		/*
		* Copyright (C) 2019 Open Source Robotics Foundation

	* Copyright (C) 2019 Open Source Robotics Foundation
	* Copyright (C) 2024 Open Source Robotics Foundation

	// Update link linear velocity
	// Update link linear velocity

	if(rootLinkPtr != linkPtrPhys){
	if(rootLinkPtr != linkPtrPhys)
	{

+                    }
+                  }
+                     }

	std::vector<Entity> entitiesVelocityReset;
	_ecm.Each<components::JointVelocityReset>(
	[&](const Entity &_entity, components::JointVelocityReset *) -> bool
	{
	entitiesVelocityReset.push_back(_entity);
	return true;
	});

	for (const auto entity : entitiesVelocityReset)
	{
	_ecm.RemoveComponent<components::JointVelocityReset>(entity);
	}

Set link velocity from set_model_state plugin #2440

Set link velocity from set_model_state plugin #2440

Conversation

yaswanth1701 commented Jun 8, 2024 • edited Loading

🎉 New feature

Summary

Test it

Checklist

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

yaswanth1701 commented Jun 11, 2024

Choose a reason for hiding this comment

scpeters left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yaswanth1701 Jun 29, 2024 • edited Loading

Choose a reason for hiding this comment

scpeters commented Jul 19, 2024

yaswanth1701 commented Jul 20, 2024

scpeters commented Aug 15, 2024

scpeters left a comment

Choose a reason for hiding this comment

Set link velocity from `set_model_state` plugin #2440

Set link velocity from `set_model_state` plugin #2440

yaswanth1701 commented Jun 8, 2024 •

edited

Loading

yaswanth1701 Jun 29, 2024 •

edited

Loading