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

SimbodyLink: add and set force and torque #3262

Open
wants to merge 2 commits into
base: gazebo11
Choose a base branch
from
Open

SimbodyLink: add and set force and torque #3262

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ba7000a
Implement methods for adding and setting forces and torques in simbody
jeduardo211 Sep 10, 2022
725eb01
Fix SimbodyLink::AddRelativeForce
jeduardo211 Sep 22, 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
71 changes: 52 additions & 19 deletions gazebo/physics/simbody/SimbodyLink.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -440,11 +440,15 @@ ignition::math::Vector3d SimbodyLink::WorldAngularVel() const
//////////////////////////////////////////////////
void SimbodyLink::SetForce(const ignition::math::Vector3d &_force)
{
SimTK::Vec3 f(SimbodyPhysics::Vector3ToVec3(_force));
auto wrench = this->simbodyPhysics->discreteForces.getOneBodyForce(
this->simbodyPhysics->integ->getState(), this->masterMobod);

// Set translational component
wrench[1] = SimbodyPhysics::Vector3ToVec3(_force);

this->simbodyPhysics->discreteForces.setOneBodyForce(
this->simbodyPhysics->integ->updAdvancedState(),
this->masterMobod, SimTK::SpatialVec(SimTK::Vec3(0), f));
this->masterMobod, wrench);
}

//////////////////////////////////////////////////
Expand All @@ -462,11 +466,15 @@ ignition::math::Vector3d SimbodyLink::WorldForce() const
//////////////////////////////////////////////////
void SimbodyLink::SetTorque(const ignition::math::Vector3d &_torque)
{
SimTK::Vec3 t(SimbodyPhysics::Vector3ToVec3(_torque));
auto wrench = this->simbodyPhysics->discreteForces.getOneBodyForce(
this->simbodyPhysics->integ->getState(), this->masterMobod);

// Set rotational component
wrench[0] = SimbodyPhysics::Vector3ToVec3(_torque);

this->simbodyPhysics->discreteForces.setOneBodyForce(
this->simbodyPhysics->integ->updAdvancedState(),
this->masterMobod, SimTK::SpatialVec(t, SimTK::Vec3(0)));
this->masterMobod, wrench);
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -505,44 +513,69 @@ void SimbodyLink::AddForce(const ignition::math::Vector3d &_force)
}

/////////////////////////////////////////////////
void SimbodyLink::AddRelativeForce(const ignition::math::Vector3d &/*_force*/)
void SimbodyLink::AddRelativeForce(const ignition::math::Vector3d &_force)
{
gzerr << "Not implemented.\n";
auto const worldForce = this->WorldPose().Rot().RotateVector(_force);
this->AddTorque(worldForce);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm confused why we would call AddTorque from the AddRelativeForce method

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm sorry, this is a mistake I made. Line 519 should be a call to this->AddForce. I fixed this in the latest commit.

}

/////////////////////////////////////////////////
void SimbodyLink::AddForceAtWorldPosition(
const ignition::math::Vector3d &/*_force*/,
const ignition::math::Vector3d &/*_pos*/)
const ignition::math::Vector3d &_force,
const ignition::math::Vector3d &_pos)
{
gzerr << "Not implemented.\n";
auto const relPos = this->WorldPose().Inverse().CoordPositionAdd(_pos);
this->AddForceAtRelativePosition(_force, relPos);
}

/////////////////////////////////////////////////
void SimbodyLink::AddForceAtRelativePosition(
const ignition::math::Vector3d &/*_force*/,
const ignition::math::Vector3d &/*_relpos*/)
const ignition::math::Vector3d &_force,
const ignition::math::Vector3d &_relpos)
{
gzerr << "Not implemented.\n";
auto const force = SimbodyPhysics::Vector3ToVec3(_force);
auto const position = SimbodyPhysics::Vector3ToVec3(_relpos);

this->simbodyPhysics->discreteForces.addForceToBodyPoint(
this->simbodyPhysics->integ->updAdvancedState(), this->masterMobod,
position,
force);
}

//////////////////////////////////////////////////
void SimbodyLink::AddLinkForce(const ignition::math::Vector3d &/*_force*/,
const ignition::math::Vector3d &/*_offset*/)
void SimbodyLink::AddLinkForce(const ignition::math::Vector3d &_force,
const ignition::math::Vector3d &_offset)
{
gzlog << "SimbodyLink::AddLinkForce not yet implemented (issue #1478)."
<< std::endl;
auto const worldForce = SimbodyPhysics::Vector3ToVec3(
this->WorldPose().Rot().RotateVector(_force));
auto const position = SimbodyPhysics::Vector3ToVec3(_offset -
this->inertial->CoG());

this->simbodyPhysics->discreteForces.addForceToBodyPoint(
this->simbodyPhysics->integ->updAdvancedState(), this->masterMobod,
position,
worldForce);
}

/////////////////////////////////////////////////
void SimbodyLink::AddTorque(const ignition::math::Vector3d &/*_torque*/)
void SimbodyLink::AddTorque(const ignition::math::Vector3d &_torque)
{
auto wrench = this->simbodyPhysics->discreteForces.getOneBodyForce(
this->simbodyPhysics->integ->getState(), this->masterMobod);

// Set rotational component
wrench[0] += SimbodyPhysics::Vector3ToVec3(_torque);

this->simbodyPhysics->discreteForces.setOneBodyForce(
this->simbodyPhysics->integ->updAdvancedState(),
this->masterMobod, wrench);
}

/////////////////////////////////////////////////
void SimbodyLink::AddRelativeTorque(const ignition::math::Vector3d &/*_torque*/)
void SimbodyLink::AddRelativeTorque(const ignition::math::Vector3d &_torque)
{
gzerr << "Not implemented.\n";
auto const worldTorque = this->WorldPose().Rot().RotateVector(_torque);
this->AddTorque(worldTorque);
}

/////////////////////////////////////////////////
Expand Down
35 changes: 26 additions & 9 deletions test/integration/physics_link.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ void PhysicsLinkTest::AddLinkForceTwoWays(
// Note: This step must be performed after checking the link forces when DART
// or bullet is the physics engine, because otherwise the accelerations used
// by the previous tests will be cleared out before they can be tested.
if ("dart" == _physicsEngine || "bullet" == _physicsEngine)
if ("ode" != _physicsEngine)
{
_world->Step(1);
}
Expand Down Expand Up @@ -210,14 +210,6 @@ void PhysicsLinkTest::AddLinkForceTwoWays(
/////////////////////////////////////////////////
void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
{
// TODO simbody currently fail this test
if (_physicsEngine == "simbody")
{
gzerr << "Aborting AddForce test for Simbody. "
<< "See issue #1478." << std::endl;
return;
}

Load("worlds/blank.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
Expand Down Expand Up @@ -258,13 +250,23 @@ void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
model->SetLinkWorldPose(ignition::math::Pose3d(
ignition::math::Vector3d(2, 3, 4),
ignition::math::Quaterniond(0, IGN_PI/2.0, 1)), link);
// Simbody requires a step to update the link pose
if(_physicsEngine == "simbody")
{
world->Step(1);
}
EXPECT_NE(ignition::math::Pose3d::Zero, link->WorldPose());
EXPECT_EQ(link->WorldPose(), link->WorldInertialPose());
this->AddLinkForceTwoWays(_physicsEngine, false, true, world, link,
ignition::math::Vector3d(-1, 10, 5));

gzdbg << "World == link == inertial frames, with offset" << std::endl;
model->SetLinkWorldPose(ignition::math::Pose3d::Zero, link);
// Simbody requires a step to update the link pose
if(_physicsEngine == "simbody")
{
world->Step(1);
}
EXPECT_EQ(ignition::math::Pose3d::Zero, link->WorldPose());
EXPECT_EQ(ignition::math::Pose3d::Zero, link->WorldInertialPose());
this->AddLinkForceTwoWays(_physicsEngine, true, true, world, link,
Expand All @@ -278,6 +280,11 @@ void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
ignition::math::Quaterniond(IGN_PI/3.0, IGN_PI*1.5, IGN_PI/4));
link->GetInertial()->SetCoG(inertialPose);
link->UpdateMass();
// Simbody requires a step to update the link pose
if(_physicsEngine == "simbody")
{
world->Step(1);
}
EXPECT_EQ(ignition::math::Pose3d::Zero, link->WorldPose());
EXPECT_EQ(inertialPose, link->WorldInertialPose());
this->AddLinkForceTwoWays(_physicsEngine, true, false, world, link,
Expand All @@ -291,6 +298,11 @@ void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
ignition::math::Quaterniond(0, 2.0*IGN_PI, IGN_PI/3));
link->GetInertial()->SetCoG(inertialPose);
link->UpdateMass();
// Simbody requires a step to update the link pose
if(_physicsEngine == "simbody")
{
world->Step(1);
}
this->AddLinkForceTwoWays(_physicsEngine, false, false, world, link,
ignition::math::Vector3d(1, 2, 1),
ignition::math::Vector3d(-2, 0.5, 1));
Expand All @@ -305,6 +317,11 @@ void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
ignition::math::Quaterniond(IGN_PI/9, 0, IGN_PI*3));
link->GetInertial()->SetCoG(inertialPose);
link->UpdateMass();
// Simbody requires a step to update the link pose
if(_physicsEngine == "simbody")
{
world->Step(1);
}
link->SetLinearVel(ignition::math::Vector3d(2, -0.1, 5));
link->SetAngularVel(ignition::math::Vector3d(-IGN_PI/10, 0, 0.0001));
this->AddLinkForceTwoWays(_physicsEngine, false, false, world, link,
Expand Down