diff --git a/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/physics/simbody/SimbodyLink.cc index 7f4cc98c89..09e9408081 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->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); } ///////////////////////////////////////////////// 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,