From ba7000abcfd5a3f0759e8ff6d25e33cb858a211d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=BAnio=20Eduardo?= Date: Sat, 10 Sep 2022 12:39:22 -0300 Subject: [PATCH 1/2] Implement methods for adding and setting forces and torques in simbody The following methods are now implemented in the SimbodyLink class: - AddForce - AddRelativeForce - AddForceAtWorldPosition - AddForceAtRelativePosition - AddLinkForce - AddTorque - AddRelativeTorque Additionally, the implementation of the SetForce, and SetTorque methods was modified to ensure that setting the force applied to the body do not alter the torque currently applied to the body and vice versa. In order to test the above methods, the AddForce test of the physics_link integration tests is now enabled for Simbody. Solves issue #1478. --- gazebo/physics/simbody/SimbodyLink.cc | 71 ++++++++++++++++++++------- test/integration/physics_link.cc | 35 +++++++++---- 2 files changed, 78 insertions(+), 28 deletions(-) diff --git a/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/physics/simbody/SimbodyLink.cc index 7f4cc98c89..b1415dd88d 100644 --- a/gazebo/physics/simbody/SimbodyLink.cc +++ b/gazebo/physics/simbody/SimbodyLink.cc @@ -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); } ////////////////////////////////////////////////// @@ -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); } ////////////////////////////////////////////////// @@ -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); } ///////////////////////////////////////////////// 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); } ///////////////////////////////////////////////// diff --git a/test/integration/physics_link.cc b/test/integration/physics_link.cc index ab0a73f65e..e4e3a21ad1 100644 --- a/test/integration/physics_link.cc +++ b/test/integration/physics_link.cc @@ -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); } @@ -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); @@ -258,6 +250,11 @@ 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, @@ -265,6 +262,11 @@ void PhysicsLinkTest::AddForce(const std::string &_physicsEngine) 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, @@ -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, @@ -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)); @@ -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, From 725eb01239990b47565f5f088cfc9df40f9c656f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=BAnio=20Eduardo=20de=20Morais=20Aquino?= Date: Thu, 22 Sep 2022 20:22:25 -0300 Subject: [PATCH 2/2] Fix SimbodyLink::AddRelativeForce The implementation of the SimbodyLink::AddRelativeForce was incorrectly calling the AddTorque method. This commit changes the implementation to call AddForce instead. --- gazebo/physics/simbody/SimbodyLink.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/physics/simbody/SimbodyLink.cc index b1415dd88d..09e9408081 100644 --- a/gazebo/physics/simbody/SimbodyLink.cc +++ b/gazebo/physics/simbody/SimbodyLink.cc @@ -516,7 +516,7 @@ void SimbodyLink::AddForce(const ignition::math::Vector3d &_force) void SimbodyLink::AddRelativeForce(const ignition::math::Vector3d &_force) { auto const worldForce = this->WorldPose().Rot().RotateVector(_force); - this->AddTorque(worldForce); + this->AddForce(worldForce); } /////////////////////////////////////////////////