Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds a Link::SetLinearVelocity() method #1323

Merged
merged 3 commits into from
Feb 8, 2022
Merged

Adds a Link::SetLinearVelocity() method #1323

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
28e212c
Adds a `Link::SetLinearVelocity()` method
arjo129 Feb 7, 2022
b93d6c4
Merge branch 'ign-gazebo6' into arjo/feat/set_link_velocity
arjo129 Feb 8, 2022
6ab486f
Updated docstring
arjo129 Feb 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions include/ignition/gazebo/Link.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,6 +220,13 @@ namespace ignition
public: void EnableVelocityChecks(EntityComponentManager &_ecm,
bool _enable = true) const;

/// \brief Set the linear velocity on this link. If this is set, wrenches
/// on this link will be ignored for the current time step.
/// \param[in] _ecm Entity Component manager.
/// \param[in] _vel Linear velocity to set in Link's Frame.
public: void SetLinearVelocity(EntityComponentManager &_ecm,
const math::Vector3d &_vel) const;

/// \brief Get the angular acceleration of the body in the world frame.
/// \param[in] _ecm Entity-component manager.
/// \return Angular acceleration of the body in the world frame or
Expand Down
20 changes: 20 additions & 0 deletions src/Link.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "ignition/gazebo/components/Joint.hh"
#include "ignition/gazebo/components/LinearAcceleration.hh"
#include "ignition/gazebo/components/LinearVelocity.hh"
#include "ignition/gazebo/components/LinearVelocityCmd.hh"
#include "ignition/gazebo/components/Link.hh"
#include "ignition/gazebo/components/Model.hh"
#include "ignition/gazebo/components/Name.hh"
Expand Down Expand Up @@ -246,6 +247,25 @@ void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable)
_enable);
}

//////////////////////////////////////////////////
void Link::SetLinearVelocity(EntityComponentManager &_ecm,
const math::Vector3d &_vel) const
{
auto vel =
_ecm.Component<components::LinearVelocityCmd>(this->dataPtr->id);

if (vel == nullptr)
{
_ecm.CreateComponent(
this->dataPtr->id,
components::LinearVelocityCmd(_vel));
}
else
{
vel->Data() = _vel;
}
}

//////////////////////////////////////////////////
std::optional<math::Vector3d> Link::WorldAngularAcceleration(
const EntityComponentManager &_ecm) const
Expand Down
34 changes: 34 additions & 0 deletions test/integration/link.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include <ignition/gazebo/components/Joint.hh>
#include <ignition/gazebo/components/LinearAcceleration.hh>
#include <ignition/gazebo/components/LinearVelocity.hh>
#include <ignition/gazebo/components/LinearVelocityCmd.hh>
#include <ignition/gazebo/components/Link.hh>
#include <ignition/gazebo/components/Model.hh>
#include <ignition/gazebo/components/Name.hh>
Expand Down Expand Up @@ -468,6 +469,39 @@ TEST_F(LinkIntegrationTest, LinkKineticEnergy)
EXPECT_DOUBLE_EQ(100.0, *link.WorldKineticEnergy(ecm));
}

//////////////////////////////////////////////////
TEST_F(LinkIntegrationTest, LinkSetVelocity)
{
EntityComponentManager ecm;
EventManager eventMgr;
SdfEntityCreator creator(ecm, eventMgr);

auto eLink = ecm.CreateEntity();
ecm.CreateComponent(eLink, components::Link());

Link link(eLink);
EXPECT_EQ(eLink, link.Entity());

ASSERT_TRUE(link.Valid(ecm));

// No LinearVelocityCmd should exist by default
EXPECT_EQ(nullptr, ecm.Component<components::LinearVelocityCmd>(eLink));

math::Vector3d linVel(1, 0, 0);
link.SetLinearVelocity(ecm, linVel);

// LinearVelocity cmd should exist
EXPECT_NE(nullptr, ecm.Component<components::LinearVelocityCmd>(eLink));
EXPECT_EQ(linVel,
ecm.Component<components::LinearVelocityCmd>(eLink)->Data());

// Make sure the linear velocity is updated
math::Vector3d linVel2(0, 0, 0);
link.SetLinearVelocity(ecm, linVel2);
EXPECT_EQ(linVel2,
ecm.Component<components::LinearVelocityCmd>(eLink)->Data());
}

//////////////////////////////////////////////////
TEST_F(LinkIntegrationTest, LinkAddWorldForce)
{
Expand Down