Skip to content

Commit

Permalink
BulletLink: add and set force and torque (#3255)
Browse files Browse the repository at this point in the history 
The following methods are now implemented in BulletLink:

- AddForce
- AddRelativeForce
- AddForceAtWorldPosition
- AddForceAtRelativePosition
- AddLinkForce
- AddTorque
- AddRelativeTorque
- SetForce
- SetTorque
- WorldTorque

Additionally, the UpdateMass method was modified to also
update the CoG position.

In order to test the above methods, the AddForce test of the
physics_link integration tests is now enabled for Bullet.

Solves issue #1476.
  • Loading branch information
@jeduardo211
jeduardo211 authored Sep 14, 2022
1 parent bf205dd commit 0e8e34d
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 56 deletions.
97 changes: 61 additions & 36 deletions gazebo/physics/bullet/BulletLink.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -242,6 +242,8 @@ void BulletLink::UpdateMass()
}
this->rigidLink->setMassProps(this->inertial->Mass(),
BulletTypes::ConvertVector3(this->inertial->PrincipalMoments()));
// In case the CoG position changed
this->OnPoseChange();
}
}

Expand Down Expand Up @@ -466,21 +468,19 @@ void BulletLink::SetForce(const ignition::math::Vector3d &_force)
if (!this->rigidLink)
return;

this->rigidLink->applyCentralForce(
btVector3(_force.X(), _force.Y(), _force.Z()));
auto const forceToApply = _force - this->WorldForce();

this->rigidLink->applyCentralForce(BulletTypes::ConvertVector3(forceToApply));
}

//////////////////////////////////////////////////
ignition::math::Vector3d BulletLink::WorldForce() const
{
if (!this->rigidLink)
return ignition::math::Vector3d(0, 0, 0);

btVector3 btVec;

btVec = this->rigidLink->getTotalForce();
return ignition::math::Vector3d::Zero;

return ignition::math::Vector3d(btVec.x(), btVec.y(), btVec.z());
auto const totalForce = this->rigidLink->getTotalForce();
return BulletTypes::ConvertVector3Ign(totalForce);
}

//////////////////////////////////////////////////
Expand All @@ -493,19 +493,18 @@ void BulletLink::SetTorque(const ignition::math::Vector3d &_torque)
return;
}

this->rigidLink->applyTorque(BulletTypes::ConvertVector3(_torque));
auto const torqueToApply = _torque - this->WorldTorque();

this->rigidLink->applyTorque(BulletTypes::ConvertVector3(torqueToApply));
}

//////////////////////////////////////////////////
ignition::math::Vector3d BulletLink::WorldTorque() const
{
// if (!this->rigidLink)
// return ignition::math::Vector3d(0, 0, 0);
// btVector3 btVec;
// btVec = this->rigidLink->getTotalTorque();
// return ignition::math::Vector3d(btVec.x(), btVec.y(), btVec.z());
if (!this->rigidLink)
return ignition::math::Vector3d::Zero;

return ignition::math::Vector3d::Zero;
return BulletTypes::ConvertVector3Ign(this->rigidLink->getTotalTorque());
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -608,53 +607,79 @@ void BulletLink::SetAngularDamping(double _damping)
// }

/////////////////////////////////////////////////
void BulletLink::AddForce(const ignition::math::Vector3d &/*_force*/)
void BulletLink::AddForce(const ignition::math::Vector3d &_force)
{
gzlog << "BulletLink::AddForce not yet implemented." << std::endl;
if (!this->rigidLink)
return;

this->rigidLink->applyCentralForce(BulletTypes::ConvertVector3(_force));
}

/////////////////////////////////////////////////
void BulletLink::AddRelativeForce(const ignition::math::Vector3d &/*_force*/)
void BulletLink::AddRelativeForce(const ignition::math::Vector3d &_force)
{
gzlog << "BulletLink::AddRelativeForce not yet implemented." << std::endl;
auto const worldForce = this->WorldPose().Rot().RotateVector(_force);
this->AddForce(worldForce);
}

/////////////////////////////////////////////////
void BulletLink::AddForceAtWorldPosition(
const ignition::math::Vector3d &/*_force*/,
const ignition::math::Vector3d &/*_pos*/)
void BulletLink::AddForceAtWorldPosition(const ignition::math::Vector3d &_force,
const ignition::math::Vector3d &_pos)
{
gzlog << "BulletLink::AddForceAtWorldPosition not yet implemented."
<< std::endl;
if (!this->rigidLink)
return;

this->rigidLink->applyForce(BulletTypes::ConvertVector3(_force),
BulletTypes::ConvertVector3(_pos));
}

/////////////////////////////////////////////////
void BulletLink::AddForceAtRelativePosition(
const ignition::math::Vector3d &/*_force*/,
const ignition::math::Vector3d &/*_relpos*/)
const ignition::math::Vector3d &_force,
const ignition::math::Vector3d &_relpos)
{
gzlog << "BulletLink::AddForceAtRelativePosition not yet implemented."
<< std::endl;
if (!this->rigidLink)
return;

// Compute the offset in the world frame
auto const cogOffset = this->WorldPose().Rot().RotateVector(_relpos);

this->rigidLink->applyForce(BulletTypes::ConvertVector3(_force),
BulletTypes::ConvertVector3(cogOffset));
}

//////////////////////////////////////////////////
void BulletLink::AddLinkForce(const ignition::math::Vector3d &/*_force*/,
const ignition::math::Vector3d &/*_offset*/)
void BulletLink::AddLinkForce(const ignition::math::Vector3d &_force,
const ignition::math::Vector3d &_offset)
{
gzlog << "BulletLink::AddLinkForce not yet implemented (#1476)."
<< std::endl;
if (!this->rigidLink)
return;

// Convert the force to the world frame
auto const worldOrientation = this->WorldPose().Rot();
auto const worldForce = worldOrientation.RotateVector(_force);
// Compute offset relative to the CoG
auto const cogOffset = worldOrientation.RotateVector(_offset
- this->inertial->CoG());

this->rigidLink->applyForce(BulletTypes::ConvertVector3(worldForce),
BulletTypes::ConvertVector3(cogOffset));
}

/////////////////////////////////////////////////
void BulletLink::AddTorque(const ignition::math::Vector3d &/*_torque*/)
void BulletLink::AddTorque(const ignition::math::Vector3d &_torque)
{
gzlog << "BulletLink::AddTorque not yet implemented." << std::endl;
if (!this->rigidLink)
return;

this->rigidLink->applyTorque(BulletTypes::ConvertVector3(_torque));
}

/////////////////////////////////////////////////
void BulletLink::AddRelativeTorque(const ignition::math::Vector3d &/*_torque*/)
void BulletLink::AddRelativeTorque(const ignition::math::Vector3d &_torque)
{
gzlog << "BulletLink::AddRelativeTorque not yet implemented." << std::endl;
auto const worldTorque = this->WorldPose().Rot().RotateVector(_torque);
this->AddTorque(worldTorque);
}

/////////////////////////////////////////////////
Expand Down
39 changes: 19 additions & 20 deletions test/integration/physics_link.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,39 +120,39 @@ void PhysicsLinkTest::AddLinkForceTwoWays(
}

// Check force and torque relative to the COG in world coordinates
ignition::math::Vector3d forceWorld = poseWorld0.Rot().RotateVector(_force);
ignition::math::Vector3d const forceWorld = poseWorld0.Rot().RotateVector(
_force);
EXPECT_EQ(forceWorld, _link->WorldForce());

ignition::math::Vector3d worldOffset = poseWorld0.Rot().RotateVector(
_offset - _link->GetInertial()->CoG());
ignition::math::Vector3d const worldOffset = poseWorld0.Rot().RotateVector(
_offset - _link->GetInertial()->CoG());
ignition::math::Vector3d torqueWorld = worldOffset.Cross(forceWorld);
EXPECT_EQ(torqueWorld, _link->WorldTorque());

// Check acceleration in world frame
ignition::math::Vector3d oneStepLinearAccel =
forceWorld/_link->GetInertial()->Mass();
ignition::math::Vector3d const oneStepLinearAccel = forceWorld /
_link->GetInertial()->Mass();
EXPECT_EQ(oneStepLinearAccel, _link->WorldLinearAccel());

// Compute angular accel by multiplying world torque
// by inverse of world inertia matrix.
// In this case, the gyroscopic coupling terms are zero
// since the model is a unit box.
ignition::math::Vector3d oneStepAngularAccel =
_link->WorldInertiaMatrix().Inverse() * torqueWorld;
ignition::math::Vector3d const oneStepAngularAccel =
_link->WorldInertiaMatrix().Inverse() * torqueWorld;
EXPECT_EQ(oneStepAngularAccel, _link->WorldAngularAccel());


// Note: This step must be performed after checking the link forces when DART
// 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)
// 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)
{
_world->Step(1);
}

// Check velocity in world frame
ignition::math::Vector3d oneStepLinearVel = linearVelWorld0 +
dt*oneStepLinearAccel;
ignition::math::Vector3d const oneStepLinearVel = linearVelWorld0 +
dt * oneStepLinearAccel;

// Dev note (MXG): DART does not always produce quite the same result as the
// expected value for CoG linear velocity. It might be worth investigating
Expand All @@ -170,8 +170,8 @@ void PhysicsLinkTest::AddLinkForceTwoWays(

VEC_EXPECT_NEAR(oneStepLinearVel, _link->WorldCoGLinearVel(), tolerance);

ignition::math::Vector3d oneStepAngularVel = angularVelWorld0 +
dt*oneStepAngularAccel;
ignition::math::Vector3d const oneStepAngularVel = angularVelWorld0 +
dt * oneStepAngularAccel;
EXPECT_EQ(oneStepAngularVel, _link->WorldAngularVel());

// Step forward and check again
Expand Down Expand Up @@ -210,12 +210,11 @@ void PhysicsLinkTest::AddLinkForceTwoWays(
/////////////////////////////////////////////////
void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
{
// TODO bullet and simbody currently fail this test
if (_physicsEngine != "ode" && _physicsEngine != "dart")
// TODO simbody currently fail this test
if (_physicsEngine == "simbody")
{
gzerr << "Aborting AddForce test for Bullet and Simbody. "
<< "See issues #1476 and #1478."
<< std::endl;
gzerr << "Aborting AddForce test for Simbody. "
<< "See issue #1478." << std::endl;
return;
}

Expand Down

0 comments on commit 0e8e34d

Please sign in to comment.