Skip to content
This repository has been archived by the owner on Feb 3, 2025. It is now read-only.

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 all commits
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->AddForce(worldForce);
}

/////////////////////////////////////////////////
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